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  {
64 
65  // Make the neighborhood radius smaller than sampling range to
66  // keep probabilities relatively high for rejection sampling
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
std::vector< Motion * > startMotions_
The set of all states in the start tree.
Definition: BiEST.h:140
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition: BiEST.h:156
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
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:264
RNG rng_
The random number generator.
Definition: BiEST.h:165
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...
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition: BiEST.h:159
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
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
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
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:175
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:68
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
double nbrhoodRadius_
The radius considered for neighborhood.
Definition: BiEST.h:162
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
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:101
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
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
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
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
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: BiEST.h:130
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
PDF< Motion * > startPdf_
The probability distribution function over states in each tree.
Definition: BiEST.h:144
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
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
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: BiEST.h:168
void setRange(double distance)
Set the range the planner is supposed to use.
Definition: BiEST.h:82
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
std::shared_ptr< NearestNeighbors< Motion * > > nnStart_
A nearest-neighbors datastructure containing the tree of motions.
Definition: BiEST.h:136
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