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
BiEST(const base::SpaceInformationPtr & si)43 ompl::geometric::BiEST::BiEST(const base::SpaceInformationPtr &si) : base::Planner(si, "BiEST")
44 {
45 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
46 specs_.directed = true;
47
48 Planner::declareParam<double>("range", this, &BiEST::setRange, &BiEST::getRange, "0.:1.:10000.");
49 }
50
~BiEST()51 ompl::geometric::BiEST::~BiEST()
52 {
53 freeMemory();
54 }
55
setup()56 void ompl::geometric::BiEST::setup()
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
clear()81 void ompl::geometric::BiEST::clear()
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
freeMemory()100 void ompl::geometric::BiEST::freeMemory()
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
solve(const base::PlannerTerminationCondition & ptc)117 ompl::base::PlannerStatus ompl::geometric::BiEST::solve(const base::PlannerTerminationCondition &ptc)
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());
125 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
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());
143 return base::PlannerStatus::INVALID_START;
144 }
145
146 if (!goal->couldSample())
147 {
148 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
149 return base::PlannerStatus::INVALID_GOAL;
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());
276 return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
277 }
278
addMotion(Motion * motion,std::vector<Motion * > & motions,PDF<Motion * > & pdf,const std::shared_ptr<NearestNeighbors<Motion * >> & nn,const std::vector<Motion * > & neighbors)279 void 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
getPlannerData(base::PlannerData & data) const296 void ompl::geometric::BiEST::getPlannerData(base::PlannerData &data) const
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 }
322