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  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 
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 
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 }
Abstract representation of a container that can perform nearest neighbors queries.
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
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
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
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 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
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
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