BiEST.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/BiEST.h"
38#include "ompl/base/goals/GoalSampleableRegion.h"
39#include "ompl/tools/config/SelfConfig.h"
40#include <limits>
41#include <cassert>
42
43ompl::geometric::BiEST::BiEST(const base::SpaceInformationPtr &si) : base::Planner(si, "BiEST")
44{
46 specs_.directed = true;
47
48 Planner::declareParam<double>("range", this, &BiEST::setRange, &BiEST::getRange, "0.:1.:10000.");
49}
50
51ompl::geometric::BiEST::~BiEST()
52{
53 freeMemory();
54}
55
57{
58 Planner::setup();
59
60 tools::SelfConfig sc(si_, getName());
61 sc.configurePlannerRange(maxDistance_);
62
63 // Make the neighborhood radius smaller than sampling range to
64 // keep probabilities relatively high for rejection sampling
65 nbrhoodRadius_ = maxDistance_ / 3.0;
66
67 if (!nnStart_)
68 nnStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
69 if (!nnGoal_)
70 nnGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
71 nnStart_->setDistanceFunction([this](const Motion *a, const Motion *b)
72 {
73 return distanceFunction(a, b);
74 });
75 nnGoal_->setDistanceFunction([this](const Motion *a, const Motion *b)
76 {
77 return distanceFunction(a, b);
78 });
79}
80
82{
83 Planner::clear();
84 sampler_.reset();
85 freeMemory();
86 if (nnStart_)
87 nnStart_->clear();
88 if (nnGoal_)
89 nnGoal_->clear();
90
91 startMotions_.clear();
92 startPdf_.clear();
93
94 goalMotions_.clear();
95 goalPdf_.clear();
96
97 connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
98}
99
101{
102 for (auto &startMotion : startMotions_)
103 {
104 if (startMotion->state != nullptr)
105 si_->freeState(startMotion->state);
106 delete startMotion;
107 }
108
109 for (auto &goalMotion : goalMotions_)
110 {
111 if (goalMotion->state != nullptr)
112 si_->freeState(goalMotion->state);
113 delete goalMotion;
114 }
115}
116
118{
119 checkValidity();
120 auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
121
122 if (goal == nullptr)
123 {
124 OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
126 }
127
128 std::vector<Motion *> neighbors;
129
130 while (const base::State *st = pis_.nextStart())
131 {
132 auto *motion = new Motion(si_);
133 si_->copyState(motion->state, st);
134 motion->root = motion->state;
135
136 nnStart_->nearestR(motion, nbrhoodRadius_, neighbors);
137 addMotion(motion, startMotions_, startPdf_, nnStart_, neighbors);
138 }
139
140 if (startMotions_.empty())
141 {
142 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
144 }
145
146 if (!goal->couldSample())
147 {
148 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
150 }
151
152 if (!sampler_)
153 sampler_ = si_->allocValidStateSampler();
154
155 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
156 startMotions_.size() + goalMotions_.size());
157
158 base::State *xstate = si_->allocState();
159 auto *xmotion = new Motion();
160
161 bool startTree = true;
162 bool solved = false;
163
164 while (!ptc && !solved)
165 {
166 // Make sure goal tree has at least one state.
167 if (goalMotions_.empty() || pis_.getSampledGoalsCount() < goalMotions_.size() / 2)
168 {
169 const base::State *st = goalMotions_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
170 if (st != nullptr)
171 {
172 auto *motion = new Motion(si_);
173 si_->copyState(motion->state, st);
174 motion->root = motion->state;
175
176 nnGoal_->nearestR(motion, nbrhoodRadius_, neighbors);
177 addMotion(motion, goalMotions_, goalPdf_, nnGoal_, neighbors);
178 }
179
180 if (goalMotions_.empty())
181 {
182 OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
183 break;
184 }
185 }
186
187 // Pointers to the tree structure we are expanding
188 std::vector<Motion *> &motions = startTree ? startMotions_ : goalMotions_;
189 PDF<Motion *> &pdf = startTree ? startPdf_ : goalPdf_;
190 std::shared_ptr<NearestNeighbors<Motion *>> nn = startTree ? nnStart_ : nnGoal_;
191
192 // Select a state to expand from
193 Motion *existing = pdf.sample(rng_.uniform01());
194 assert(existing);
195
196 // Sample a state in the neighborhood
197 if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
198 continue;
199
200 // Compute neighborhood of candidate state
201 xmotion->state = xstate;
202 nn->nearestR(xmotion, nbrhoodRadius_, neighbors);
203
204 // reject state with probability proportional to neighborhood density
205 if (!neighbors.empty() )
206 {
207 double p = 1.0 - (1.0 / neighbors.size());
208 if (rng_.uniform01() < p)
209 continue;
210 }
211
212 // Is motion good?
213 if (si_->checkMotion(existing->state, xstate))
214 {
215 // create a motion
216 auto *motion = new Motion(si_);
217 si_->copyState(motion->state, xstate);
218 motion->parent = existing;
219 motion->root = existing->root;
220
221 // add it to everything
222 addMotion(motion, motions, pdf, nn, neighbors);
223
224 // try to connect this state to the other tree
225 // Get all states in the other tree within a maxDistance_ ball (bigger than "neighborhood" ball)
226 startTree ? nnGoal_->nearestR(motion, maxDistance_, neighbors) :
227 nnStart_->nearestR(motion, maxDistance_, neighbors);
228 for (size_t i = 0; i < neighbors.size() && !solved; ++i)
229 {
230 if (goal->isStartGoalPairValid(motion->root, neighbors[i]->root) &&
231 si_->checkMotion(motion->state, neighbors[i]->state)) // win! solution found.
232 {
233 connectionPoint_ = std::make_pair(motion->state, neighbors[i]->state);
234
235 Motion *startMotion = startTree ? motion : neighbors[i];
236 Motion *goalMotion = startTree ? neighbors[i] : motion;
237
238 Motion *solution = startMotion;
239 std::vector<Motion *> mpath1;
240 while (solution != nullptr)
241 {
242 mpath1.push_back(solution);
243 solution = solution->parent;
244 }
245
246 solution = goalMotion;
247 std::vector<Motion *> mpath2;
248 while (solution != nullptr)
249 {
250 mpath2.push_back(solution);
251 solution = solution->parent;
252 }
253
254 auto path(std::make_shared<PathGeometric>(si_));
255 path->getStates().reserve(mpath1.size() + mpath2.size());
256 for (int i = mpath1.size() - 1; i >= 0; --i)
257 path->append(mpath1[i]->state);
258 for (auto &i : mpath2)
259 path->append(i->state);
260
261 pdef_->addSolutionPath(path, false, 0.0, getName());
262 solved = true;
263 }
264 }
265 }
266
267 // swap trees for next iteration
268 startTree = !startTree;
269 }
270
271 si_->freeState(xstate);
272 delete xmotion;
273
274 OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(),
275 startMotions_.size() + goalMotions_.size(), startMotions_.size(), goalMotions_.size());
277}
278
279void ompl::geometric::BiEST::addMotion(Motion *motion, std::vector<Motion *> &motions, PDF<Motion *> &pdf,
280 const std::shared_ptr<NearestNeighbors<Motion *>> &nn,
281 const std::vector<Motion *> &neighbors)
282{
283 // Updating neighborhood size counts
284 for (auto neighbor : neighbors)
285 {
286 PDF<Motion *>::Element *elem = neighbor->element;
287 double w = pdf.getWeight(elem);
288 pdf.update(elem, w / (w + 1.));
289 }
290
291 motion->element = pdf.add(motion, 1. / (neighbors.size() + 1.)); // +1 for self
292 motions.push_back(motion);
293 nn->add(motion);
294}
295
297{
298 Planner::getPlannerData(data);
299
300 for (auto startMotion : startMotions_)
301 {
302 if (startMotion->parent == nullptr)
303 data.addStartVertex(base::PlannerDataVertex(startMotion->state, 1));
304 else
305 data.addEdge(base::PlannerDataVertex(startMotion->parent->state, 1),
306 base::PlannerDataVertex(startMotion->state, 1));
307 }
308
309 for (auto goalMotion : goalMotions_)
310 {
311 if (goalMotion->parent == nullptr)
312 data.addGoalVertex(base::PlannerDataVertex(goalMotion->state, 2));
313 else
314 // The edges in the goal tree are reversed to be consistent with start tree
315 data.addEdge(base::PlannerDataVertex(goalMotion->state, 2),
316 base::PlannerDataVertex(goalMotion->parent->state, 2));
317 }
318
319 // Add the edge connecting the two trees
320 data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
321}
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
double getWeight(const Element *elem) const
Returns the current weight of the given Element.
Definition: PDF.h:171
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
Definition: PDF.h:132
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element,...
Definition: PDF.h:97
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
Abstract definition of a goal region that can be sampled.
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,...
Definition: PlannerData.h:175
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
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...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:429
Definition of an abstract state.
Definition: State.h:50
The definition of a motion.
Definition: BiEST.h:104
base::State * state
The state contained by the motion.
Definition: BiEST.h:117
const base::State * root
The root node of the tree this motion is in.
Definition: BiEST.h:126
Motion * parent
The parent motion in the exploration tree.
Definition: BiEST.h:120
PDF< Motion * >::Element * element
A pointer to the corresponding element in the probability distribution function.
Definition: BiEST.h:123
void addMotion(Motion *motion, std::vector< Motion * > &motions, PDF< Motion * > &pdf, const std::shared_ptr< NearestNeighbors< Motion * > > &nn, const std::vector< Motion * > &neighbors)
Add a motion to the exploration tree.
Definition: BiEST.cpp:279
void freeMemory()
Free the memory allocated by this planner.
Definition: BiEST.cpp:100
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: BiEST.cpp:56
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: BiEST.cpp:117
double getRange() const
Get the range the planner is using.
Definition: BiEST.h:92
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: BiEST.cpp:81
BiEST(const base::SpaceInformationPtr &si)
Constructor.
Definition: BiEST.cpp:43
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: BiEST.h:82
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: BiEST.cpp:296
This class contains methods that automatically configure various parameters for motion planning....
Definition: SelfConfig.h:60
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
#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
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:212
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:196
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
@ INVALID_GOAL
Invalid goal state.
Definition: PlannerStatus.h:58
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62