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  maxDistance_ = 0.0;
48  connectionPoint_ = std::make_pair<ompl::base::State *, ompl::base::State *>(nullptr, nullptr);
49 
50  Planner::declareParam<double>("range", this, &BiEST::setRange, &BiEST::getRange, "0.:1.:10000.");
51 }
52 
53 ompl::geometric::BiEST::~BiEST()
54 {
55  freeMemory();
56 }
57 
59 {
60  Planner::setup();
61 
62  if (maxDistance_ < 1e-3)
63  {
66 
67  // Make the neighborhood radius smaller than sampling range to
68  // keep probabilities relatively high for rejection sampling
70  }
71 
72  if (!nnStart_)
73  nnStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
74  if (!nnGoal_)
75  nnGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
76  nnStart_->setDistanceFunction([this](const Motion *a, const Motion *b)
77  {
78  return distanceFunction(a, b);
79  });
80  nnGoal_->setDistanceFunction([this](const Motion *a, const Motion *b)
81  {
82  return distanceFunction(a, b);
83  });
84 }
85 
87 {
88  Planner::clear();
89  sampler_.reset();
90  freeMemory();
91  if (nnStart_)
92  nnStart_->clear();
93  if (nnGoal_)
94  nnGoal_->clear();
95 
96  startMotions_.clear();
97  startPdf_.clear();
98 
99  goalMotions_.clear();
100  goalPdf_.clear();
101 
102  connectionPoint_ = std::make_pair<base::State *, base::State *>(nullptr, nullptr);
103 }
104 
106 {
107  for (auto &startMotion : startMotions_)
108  {
109  if (startMotion->state)
110  si_->freeState(startMotion->state);
111  delete startMotion;
112  }
113 
114  for (auto &goalMotion : goalMotions_)
115  {
116  if (goalMotion->state)
117  si_->freeState(goalMotion->state);
118  delete goalMotion;
119  }
120 }
121 
123 {
124  checkValidity();
125  base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
126 
127  if (!goal)
128  {
129  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
131  }
132 
133  std::vector<Motion *> neighbors;
134 
135  while (const base::State *st = pis_.nextStart())
136  {
137  auto *motion = new Motion(si_);
138  si_->copyState(motion->state, st);
139  motion->root = motion->state;
140 
141  nnStart_->nearestR(motion, nbrhoodRadius_, neighbors);
142  addMotion(motion, startMotions_, startPdf_, nnStart_, neighbors);
143  }
144 
145  if (startMotions_.size() == 0)
146  {
147  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
149  }
150 
151  if (!goal->couldSample())
152  {
153  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
155  }
156 
157  if (!sampler_)
158  sampler_ = si_->allocValidStateSampler();
159 
160  OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
161  startMotions_.size() + goalMotions_.size());
162 
163  base::State *xstate = si_->allocState();
164  auto *xmotion = new Motion();
165 
166  bool startTree = true;
167  bool solved = false;
168 
169  while (ptc == false && !solved)
170  {
171  // Make sure goal tree has at least one state.
172  if (goalMotions_.size() == 0 || pis_.getSampledGoalsCount() < goalMotions_.size() / 2)
173  {
174  const base::State *st = goalMotions_.size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
175  if (st)
176  {
177  auto *motion = new Motion(si_);
178  si_->copyState(motion->state, st);
179  motion->root = motion->state;
180 
181  nnGoal_->nearestR(motion, nbrhoodRadius_, neighbors);
182  addMotion(motion, goalMotions_, goalPdf_, nnGoal_, neighbors);
183  }
184 
185  if (goalMotions_.size() == 0)
186  {
187  OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
188  break;
189  }
190  }
191 
192  // Pointers to the tree structure we are expanding
193  std::vector<Motion *> &motions = startTree ? startMotions_ : goalMotions_;
194  PDF<Motion *> &pdf = startTree ? startPdf_ : goalPdf_;
195  std::shared_ptr<NearestNeighbors<Motion *>> nn = startTree ? nnStart_ : nnGoal_;
196 
197  // Select a state to expand from
198  Motion *existing = pdf.sample(rng_.uniform01());
199  assert(existing);
200 
201  // Sample a state in the neighborhood
202  if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
203  continue;
204 
205  // Compute neighborhood of candidate state
206  xmotion->state = xstate;
207  nn->nearestR(xmotion, nbrhoodRadius_, neighbors);
208 
209  // reject state with probability proportional to neighborhood density
210  if (neighbors.size())
211  {
212  double p = 1.0 - (1.0 / neighbors.size());
213  if (rng_.uniform01() < p)
214  continue;
215  }
216 
217  // Is motion good?
218  if (si_->checkMotion(existing->state, xstate))
219  {
220  // create a motion
221  auto *motion = new Motion(si_);
222  si_->copyState(motion->state, xstate);
223  motion->parent = existing;
224  motion->root = existing->root;
225 
226  // add it to everything
227  addMotion(motion, motions, pdf, nn, neighbors);
228 
229  // try to connect this state to the other tree
230  // Get all states in the other tree within a maxDistance_ ball (bigger than "neighborhood" ball)
231  startTree ? nnGoal_->nearestR(motion, maxDistance_, neighbors) :
232  nnStart_->nearestR(motion, maxDistance_, neighbors);
233  for (size_t i = 0; i < neighbors.size() && !solved; ++i)
234  {
235  if (goal->isStartGoalPairValid(motion->root, neighbors[i]->root) &&
236  si_->checkMotion(motion->state, neighbors[i]->state)) // win! solution found.
237  {
238  connectionPoint_ = std::make_pair(motion->state, neighbors[i]->state);
239 
240  Motion *startMotion = startTree ? motion : neighbors[i];
241  Motion *goalMotion = startTree ? neighbors[i] : motion;
242 
243  Motion *solution = startMotion;
244  std::vector<Motion *> mpath1;
245  while (solution != nullptr)
246  {
247  mpath1.push_back(solution);
248  solution = solution->parent;
249  }
250 
251  solution = goalMotion;
252  std::vector<Motion *> mpath2;
253  while (solution != nullptr)
254  {
255  mpath2.push_back(solution);
256  solution = solution->parent;
257  }
258 
259  auto path(std::make_shared<PathGeometric>(si_));
260  path->getStates().reserve(mpath1.size() + mpath2.size());
261  for (int i = mpath1.size() - 1; i >= 0; --i)
262  path->append(mpath1[i]->state);
263  for (auto &i : mpath2)
264  path->append(i->state);
265 
266  pdef_->addSolutionPath(path, false, 0.0, getName());
267  solved = true;
268  }
269  }
270  }
271 
272  // swap trees for next iteration
273  startTree = !startTree;
274  }
275 
276  si_->freeState(xstate);
277  delete xmotion;
278 
279  OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(),
280  startMotions_.size() + goalMotions_.size(), startMotions_.size(), goalMotions_.size());
282 }
283 
284 void ompl::geometric::BiEST::addMotion(Motion *motion, std::vector<Motion *> &motions, PDF<Motion *> &pdf,
285  const std::shared_ptr<NearestNeighbors<Motion *>> &nn,
286  const std::vector<Motion *> &neighbors)
287 {
288  // Updating neighborhood size counts
289  for (auto neighbor : neighbors)
290  {
291  PDF<Motion *>::Element *elem = neighbor->element;
292  double w = pdf.getWeight(elem);
293  pdf.update(elem, w / (w + 1.));
294  }
295 
296  motion->element = pdf.add(motion, 1. / (neighbors.size() + 1.)); // +1 for self
297  motions.push_back(motion);
298  nn->add(motion);
299 }
300 
302 {
303  Planner::getPlannerData(data);
304 
305  for (auto startMotion : startMotions_)
306  {
307  if (startMotion->parent == nullptr)
308  data.addStartVertex(base::PlannerDataVertex(startMotion->state, 1));
309  else
310  data.addEdge(base::PlannerDataVertex(startMotion->parent->state, 1),
311  base::PlannerDataVertex(startMotion->state, 1));
312  }
313 
314  for (auto goalMotion : goalMotions_)
315  {
316  if (goalMotion->parent == nullptr)
317  data.addGoalVertex(base::PlannerDataVertex(goalMotion->state, 2));
318  else
319  // The edges in the goal tree are reversed to be consistent with start tree
320  data.addEdge(base::PlannerDataVertex(goalMotion->state, 2),
321  base::PlannerDataVertex(goalMotion->parent->state, 2));
322  }
323 
324  // Add the edge connecting the two trees
325  data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
326 }
PDF< Motion * >::Element * element
A pointer to the corresponding element in the probability distribution function.
Definition: BiEST.h:125
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:142
base::ValidStateSamplerPtr sampler_
Valid state sampler.
Definition: BiEST.h:158
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:206
double getRange() const
Get the range the planner is using.
Definition: BiEST.h:92
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
Motion * parent
The parent motion in the exploration tree.
Definition: BiEST.h:122
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:269
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
Definition: Goal.h:136
RNG rng_
The random number generator.
Definition: BiEST.h:167
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:161
const base::State * root
The root node of the tree this motion is in.
Definition: BiEST.h:128
void freeMemory()
Free the memory allocated by this planner.
Definition: BiEST.cpp:105
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:222
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:58
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:119
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:122
BiEST(const base::SpaceInformationPtr &si)
Constructor.
Definition: BiEST.cpp:43
double nbrhoodRadius_
The radius considered for neighborhood.
Definition: BiEST.h:164
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:421
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:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
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:301
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:284
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: BiEST.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, 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:146
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:86
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: BiEST.h:170
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:415
std::shared_ptr< NearestNeighbors< Motion * > > nnStart_
A nearest-neighbors datastructure containing the tree of motions.
Definition: BiEST.h:138
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