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