ImplicitGraph.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019-present University of Oxford
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 names of the copyright holders 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 // Authors: Marlin Strub
36 #include "ompl/geometric/planners/informedtrees/aitstar/ImplicitGraph.h"
37 
38 #include <cmath>
39 
40 #include <boost/math/constants/constants.hpp>
41 
42 #include "ompl/util/GeometricEquations.h"
43 
44 namespace ompl
45 {
46  namespace geometric
47  {
48  namespace aitstar
49  {
51  : batchId_(1u), solutionCost_(solutionCost)
52  {
53  }
54 
55  void ImplicitGraph::setup(const ompl::base::SpaceInformationPtr &spaceInformation,
56  const ompl::base::ProblemDefinitionPtr &problemDefinition,
57  ompl::base::PlannerInputStates *inputStates)
58  {
59  vertices_.setDistanceFunction(
60  [this](const std::shared_ptr<Vertex> &a, const std::shared_ptr<Vertex> &b) {
61  return spaceInformation_->distance(a->getState(), b->getState());
62  });
63  spaceInformation_ = spaceInformation;
64  problemDefinition_ = problemDefinition;
65  objective_ = problemDefinition->getOptimizationObjective();
66  k_rgg_ = boost::math::constants::e<double>() +
67  (boost::math::constants::e<double>() / spaceInformation->getStateDimension());
69  }
70 
71  void ImplicitGraph::clear()
72  {
73  batchId_ = 1u;
74  radius_ = std::numeric_limits<double>::infinity();
75  numNeighbors_ = std::numeric_limits<std::size_t>::max();
76  vertices_.clear();
77  startVertices_.clear();
78  goalVertices_.clear();
79  prunedStartVertices_.clear();
80  prunedGoalVertices_.clear();
81  numSampledStates_ = 0u;
82  numValidSamples_ = 0u;
83  }
84 
85  void ImplicitGraph::setRewireFactor(double rewireFactor)
86  {
87  rewireFactor_ = rewireFactor;
88  }
89 
90  double ImplicitGraph::getRewireFactor() const
91  {
92  return rewireFactor_;
93  }
94 
95  void ImplicitGraph::setUseKNearest(bool useKNearest)
96  {
97  useKNearest_ = useKNearest;
98  }
99 
100  bool ImplicitGraph::getUseKNearest() const
101  {
102  return useKNearest_;
103  }
104 
105  void ImplicitGraph::registerStartState(const ompl::base::State *const startState)
106  {
107  // Create a vertex corresponding to this state.
108  auto startVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
109 
110  // Copy the state into the vertex's state.
111  spaceInformation_->copyState(startVertex->getState(), startState);
112 
113  // By definition, this has identity cost-to-come.
114  startVertex->setCostToComeFromStart(objective_->identityCost());
115 
116  // Add the start vertex to the set of vertices.
117  vertices_.add(startVertex);
118 
119  // Remember it as a start vertex.
120  startVertices_.emplace_back(startVertex);
121  }
122 
123  void ImplicitGraph::registerGoalState(const ompl::base::State *const goalState)
124  {
125  // Create a vertex corresponding to this state.
126  auto goalVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
127 
128  // Copy the state into the vertex's state.
129  spaceInformation_->copyState(goalVertex->getState(), goalState);
130 
131  // Add the goal vertex to the set of vertices.
132  vertices_.add(goalVertex);
133 
134  // Remember it as a goal vertex.
135  goalVertices_.emplace_back(goalVertex);
136  }
137 
138  bool ImplicitGraph::hasAStartState() const
139  {
140  return !startVertices_.empty();
141  }
142 
143  bool ImplicitGraph::hasAGoalState() const
144  {
145  return !goalVertices_.empty();
146  }
147 
148  void
150  ompl::base::PlannerInputStates *inputStates)
151  {
152  // We need to keep track whether a new goal and/or a new start has been added.
153  bool addedNewGoalState = false;
154  bool addedNewStartState = false;
155 
156  // First update the goals. We have to call inputStates->nextGoal(terminationCondition) at least once
157  // (regardless of the return value of inputStates->moreGoalStates()) in case the termination condition
158  // wants us to wait for a goal.
159  do
160  {
161  // Get a new goal. If there are none, or the underlying state is invalid this will be a nullptr.
162  auto newGoalState = inputStates->nextGoal(terminationCondition);
163 
164  // If there was a new valid goal, register it as such and remember that a goal has been added.
165  if (static_cast<bool>(newGoalState))
166  {
167  registerGoalState(newGoalState);
168  addedNewGoalState = true;
169  }
170 
171  } while (inputStates->haveMoreGoalStates());
172 
173  // Having updated the goals, we now update the starts.
174  while (inputStates->haveMoreStartStates())
175  {
176  // Get the next start. The returned pointer can be a nullptr (if the state is invalid).
177  auto newStartState = inputStates->nextStart();
178 
179  // If there is a new valid start, register it as such and remember that a start has been added.
180  if (static_cast<bool>(newStartState))
181  {
182  registerStartState(newStartState);
183  addedNewStartState = true;
184  }
185  }
186 
187  // If we added a new start and have previously pruned goals, we might want to add the goals back.
188  if (addedNewStartState && !prunedGoalVertices_.empty())
189  {
190  // Keep track of the pruned goal vertices that have been revived.
191  std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedGoals;
192 
193  // Let's see if the pruned goal is close enough to any new start to revive it..
194  for (auto it = prunedGoalVertices_.begin(); it != prunedGoalVertices_.end(); ++it)
195  {
196  // Loop over all start states to get the best cost.
197  auto heuristicCost = objective_->infiniteCost();
198  for (const auto &start : startVertices_)
199  {
200  heuristicCost = objective_->betterCost(
201  heuristicCost, objective_->motionCostHeuristic(start->getState(), (*it)->getState()));
202  }
203 
204  // If this goal can possibly improve the current solution, add it back to the graph.
205  if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
206  {
207  registerGoalState((*it)->getState());
208  addedNewGoalState = true;
209  revivedGoals.emplace_back(it);
210  }
211  }
212 
213  // Remove all revived goals from the pruned goals.
214  for (const auto &revivedGoal : revivedGoals)
215  {
216  std::iter_swap(revivedGoal, prunedGoalVertices_.rbegin());
217  prunedGoalVertices_.pop_back();
218  }
219  }
220 
221  // If we added a new goal and have previously pruned starts, we might want to add the starts back.
222  if (addedNewGoalState && !prunedStartVertices_.empty())
223  {
224  // Keep track of the pruned goal vertices that have been revived.
225  std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedStarts;
226 
227  // Let's see if the pruned start is close enough to any new goal to revive it..
228  for (auto it = prunedStartVertices_.begin(); it != prunedStartVertices_.end(); ++it)
229  {
230  // Loop over all start states to get the best cost.
231  auto heuristicCost = objective_->infiniteCost();
232  for (const auto &goal : goalVertices_)
233  {
234  heuristicCost = objective_->betterCost(
235  heuristicCost, objective_->motionCostHeuristic(goal->getState(), (*it)->getState()));
236  }
237 
238  // If this goal can possibly improve the current solution, add it back to the graph.
239  if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
240  {
241  registerStartState((*it)->getState());
242  addedNewStartState = true;
243  revivedStarts.emplace_back(it);
244  }
245  }
246 
247  // Remove all revived goals from the pruned goals.
248  for (const auto &revivedStart : revivedStarts)
249  {
250  std::iter_swap(revivedStart, prunedStartVertices_.rbegin());
251  prunedStartVertices_.pop_back();
252  }
253  }
254 
255  if (addedNewGoalState || addedNewStartState)
256  {
257  // Allocate a state sampler if we have at least one start and one goal.
258  if (!startVertices_.empty() && !goalVertices_.empty())
259  {
260  sampler_ = objective_->allocInformedStateSampler(problemDefinition_,
261  std::numeric_limits<unsigned int>::max());
262  }
263  }
264 
265  if (!goalVertices_.empty() && startVertices_.empty())
266  {
267  OMPL_WARN("AIT* (ImplicitGraph): The problem has a goal but not a start. AIT* can not find a "
268  "solution since PlannerInputStates provides no method to wait for a valid start state to "
269  "appear.");
270  }
271  }
272 
273  std::size_t ImplicitGraph::computeNumberOfSamplesInInformedSet() const
274  {
275  std::size_t numberOfSamplesInInformedSet{0u};
276  std::vector<std::shared_ptr<Vertex>> vertices;
277  vertices_.list(vertices);
278 
279  // Loop over all vertices.
280  for (const auto &vertex : vertices)
281  {
282  // Get the best cost to come from any start.
283  ompl::base::Cost bestCostToComeHeuristic = objective_->infiniteCost();
284  for (const auto &start : startVertices_)
285  {
286  auto costToComeHeuristic =
287  objective_->motionCostHeuristic(start->getState(), vertex->getState());
288  if (objective_->isCostBetterThan(costToComeHeuristic, bestCostToComeHeuristic))
289  {
290  bestCostToComeHeuristic = costToComeHeuristic;
291  }
292  }
293 
294  // Get the best cost to go to any goal.
295  ompl::base::Cost bestCostToGoHeuristic = objective_->infiniteCost();
296  for (const auto &goal : goalVertices_)
297  {
298  auto costToComeHeuristic =
299  objective_->motionCostHeuristic(vertex->getState(), goal->getState());
300  if (objective_->isCostBetterThan(costToComeHeuristic, bestCostToGoHeuristic))
301  {
302  bestCostToGoHeuristic = costToComeHeuristic;
303  }
304  }
305 
306  // If this can possibly improve the current solution, it is in the informed set.
307  if (objective_->isCostBetterThan(
308  objective_->combineCosts(bestCostToComeHeuristic, bestCostToGoHeuristic), solutionCost_))
309  {
310  ++numberOfSamplesInInformedSet;
311  }
312  }
313 
314  return numberOfSamplesInInformedSet;
315  }
316 
317  std::vector<std::shared_ptr<Vertex>> ImplicitGraph::addSamples(std::size_t numNewSamples)
318  {
319  // First get the number of samples inside the informed set.
320  auto numSamplesInInformedSet = computeNumberOfSamplesInInformedSet();
321 
322  // Create new vertices.
323  std::vector<std::shared_ptr<Vertex>> newVertices;
324  newVertices.reserve(numNewSamples);
325  while (newVertices.size() < numNewSamples)
326  {
327  // Create a new vertex.
328  newVertices.emplace_back(std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_));
329 
330  do
331  {
332  // Sample the associated state uniformly within the informed set.
333  sampler_->sampleUniform(newVertices.back()->getState(), solutionCost_);
334 
335  // Count how many states we've checked.
336  ++numSampledStates_;
337  } while (!spaceInformation_->getStateValidityChecker()->isValid(newVertices.back()->getState()));
338 
339  ++numValidSamples_;
340  }
341 
342  // Add all new vertices to the nearest neighbor structure.
343  vertices_.add(newVertices);
344 
345  auto numUniformSamplesInInformedSet =
346  numSamplesInInformedSet + numNewSamples - startVertices_.size() - goalVertices_.size();
347 
348  // We need to do some internal housekeeping.
349  ++batchId_;
350  if (useKNearest_)
351  {
352  numNeighbors_ = computeNumberOfNeighbors(numUniformSamplesInInformedSet);
353  }
354  else
355  {
356  radius_ = computeConnectionRadius(numUniformSamplesInInformedSet);
357  }
358 
359  return newVertices;
360  }
361 
362  std::size_t ImplicitGraph::getNumVertices() const
363  {
364  return vertices_.size();
365  }
366 
367  double ImplicitGraph::getConnectionRadius() const
368  {
369  return radius_;
370  }
371 
372  std::vector<std::shared_ptr<Vertex>>
373  ImplicitGraph::getNeighbors(const std::shared_ptr<Vertex> &vertex) const
374  {
375  // Return cached neighbors if available.
376  if (vertex->hasCachedNeighbors())
377  {
378  return vertex->getNeighbors();
379  }
380  else
381  {
382  ++numNearestNeighborsCalls_;
383  std::vector<std::shared_ptr<Vertex>> neighbors{};
384  if (useKNearest_)
385  {
386  vertices_.nearestK(vertex, numNeighbors_, neighbors);
387  }
388  else
389  {
390  vertices_.nearestR(vertex, radius_, neighbors);
391  }
392  vertex->cacheNeighbors(neighbors);
393  return neighbors;
394  }
395  }
396 
397  bool ImplicitGraph::isStart(const std::shared_ptr<Vertex> &vertex) const
398  {
399  for (const auto &start : startVertices_)
400  {
401  if (vertex->getId() == start->getId())
402  {
403  return true;
404  }
405  }
406  return false;
407  }
408 
409  bool ImplicitGraph::isGoal(const std::shared_ptr<Vertex> &vertex) const
410  {
411  for (const auto &goal : goalVertices_)
412  {
413  if (vertex->getId() == goal->getId())
414  {
415  return true;
416  }
417  }
418  return false;
419  }
420 
421  const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getStartVertices() const
422  {
423  return startVertices_;
424  }
425 
426  const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getGoalVertices() const
427  {
428  return goalVertices_;
429  }
430 
431  std::vector<std::shared_ptr<Vertex>> ImplicitGraph::getVertices() const
432  {
433  std::vector<std::shared_ptr<Vertex>> vertices;
434  vertices_.list(vertices);
435  return vertices;
436  }
437 
438  void ImplicitGraph::prune()
439  {
440  if (!objective_->isFinite(solutionCost_))
441  {
442  return;
443  }
444 
445  std::vector<std::shared_ptr<Vertex>> vertices;
446  vertices_.list(vertices);
447 
448  // Prepare the vector of vertices to be pruned.
449  std::vector<std::shared_ptr<Vertex>> verticesToBePruned;
450 
451  // Check each vertex whether it can be pruned.
452  for (const auto &vertex : vertices)
453  {
454  // Check if the combination of the admissible costToCome and costToGo estimates results in a path
455  // that is more expensive than the current solution.
456  if (!canPossiblyImproveSolution(vertex))
457  {
458  // We keep track of pruned start and goal vertices. This is because if the user adds start or
459  // goal states after we have pruned start or goal states, we might want to reconsider pruned
460  // start or goal states.
461  if (isGoal(vertex))
462  {
463  prunedGoalVertices_.emplace_back(vertex);
464  }
465  else if (isStart(vertex))
466  {
467  prunedStartVertices_.emplace_back(vertex);
468  }
469  verticesToBePruned.emplace_back(vertex);
470  }
471  }
472 
473  // Remove all vertices to be pruned.
474  for (const auto &vertex : verticesToBePruned)
475  {
476  // Remove it from both search trees.
477  if (vertex->hasReverseParent())
478  {
479  vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
480  vertex->resetReverseParent();
481  }
482  vertex->invalidateReverseBranch();
483  if (vertex->hasForwardParent())
484  {
485  vertex->getForwardParent()->removeFromForwardChildren(vertex->getId());
486  vertex->resetForwardParent();
487  }
488  vertex->invalidateForwardBranch();
489 
490  // Remove it from the nearest neighbor struct.
491  vertices_.remove(vertex);
492  }
493 
494  // Assert that the forward and reverse queue are empty?
495  }
496 
497  std::size_t ImplicitGraph::getNumberOfSampledStates() const
498  {
499  return numSampledStates_;
500  }
501 
502  std::size_t ImplicitGraph::getNumberOfValidSamples() const
503  {
504  return numValidSamples_;
505  }
506 
507  std::size_t ImplicitGraph::getNumberOfStateCollisionChecks() const
508  {
509  // Each sampled state is checked for collision. Only sampled states are checked for collision (number of
510  // collision checked edges don't count here.)
511  return numSampledStates_;
512  }
513 
514  std::size_t ImplicitGraph::getNumberOfNearestNeighborCalls() const
515  {
516  return numNearestNeighborsCalls_;
517  }
518 
519  double ImplicitGraph::computeConnectionRadius(std::size_t numSamples) const
520  {
521  // Define the dimension as a helper variable.
522  auto dimension = static_cast<double>(spaceInformation_->getStateDimension());
523 
524  // Compute the RRT* factor.
525  return rewireFactor_ *
526  std::pow(2.0 * (1.0 + 1.0 / dimension) *
527  (sampler_->getInformedMeasure(solutionCost_) /
528  unitNBallMeasure(spaceInformation_->getStateDimension())) *
529  (std::log(static_cast<double>(numSamples)) / static_cast<double>(numSamples)),
530  1.0 / dimension);
531 
532  // // Compute the FMT* factor.
533  // return 2.0 * rewireFactor_ *
534  // std::pow((1.0 / dimension) *
535  // (sampler_->getInformedMeasure(*solutionCost_.lock()) /
536  // unitNBallMeasure(spaceInformation_->getStateDimension())) *
537  // (std::log(static_cast<double>(numSamples)) / numSamples),
538  // 1.0 / dimension);
539  }
540 
541  std::size_t ImplicitGraph::computeNumberOfNeighbors(std::size_t numSamples) const
542  {
543  return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numSamples)));
544  }
545 
546  bool ImplicitGraph::canPossiblyImproveSolution(const std::shared_ptr<Vertex> &vertex) const
547  {
548  // Get the preferred start for this vertex.
549  auto bestCostToCome = objective_->infiniteCost();
550  for (const auto &start : startVertices_)
551  {
552  auto costToCome = objective_->motionCostHeuristic(start->getState(), vertex->getState());
553  if (objective_->isCostBetterThan(costToCome, bestCostToCome))
554  {
555  bestCostToCome = costToCome;
556  }
557  }
558 
559  // Check if the combination of the admissible costToCome and costToGo estimates results in a path
560  // that is more expensive than the current solution.
561  return objective_->isCostBetterThan(
562  objective_->combineCosts(
563  bestCostToCome, objective_->costToGo(vertex->getState(), problemDefinition_->getGoal().get())),
564  solutionCost_);
565  }
566 
567  } // namespace aitstar
568 
569  } // namespace geometric
570 
571 } // namespace ompl
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
A shared pointer wrapper for ompl::base::SpaceInformation.
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:142
Definition of an abstract state.
Definition: State.h:114
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
bool getUseKNearest() const
Get whether a k-nearest search is being used.
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, CostHelper *costHelper, SearchQueue *searchQueue, const ompl::base::Planner *plannerPtr, ompl::base::PlannerInputStates &inputStates)
Setup the ImplicitGraph, must be called before use. Does not take a copy of the PlannerInputStates,...
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:237
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition: Planner.cpp:274
A shared pointer wrapper for ompl::base::ProblemDefinition.
void updateStartAndGoalStates(ompl::base::PlannerInputStates &inputStates, const base::PlannerTerminationCondition &terminationCondition)
Adds any new goals or starts that have appeared in the problem definition to the vector of vertices a...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:355
unsigned int numSamples() const
The number of samples.
std::pair< unsigned int, unsigned int > prune(double prunedMeasure)
Prune the samples to the subproblem of the given measure. Returns the number of vertices disconnected...
double getRewireFactor() const
Get the rewiring scale factor.
ImplicitGraph(const ompl::base::Cost &solutionCost)
Constructs an implicit graph.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22