ImplicitGraph.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, 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::setMaxNumberOfGoals(unsigned int maxNumberOfGoals)
96  {
97  maxNumGoals_ = maxNumberOfGoals;
98  }
99 
100  unsigned int ImplicitGraph::getMaxNumberOfGoals() const
101  {
102  return maxNumGoals_;
103  }
104 
105  void ImplicitGraph::setUseKNearest(bool useKNearest)
106  {
107  useKNearest_ = useKNearest;
108  }
109 
110  bool ImplicitGraph::getUseKNearest() const
111  {
112  return useKNearest_;
113  }
114 
115  void ImplicitGraph::registerStartState(const ompl::base::State *const startState)
116  {
117  // Create a vertex corresponding to this state.
118  auto startVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
119 
120  // Copy the state into the vertex's state.
121  spaceInformation_->copyState(startVertex->getState(), startState);
122 
123  // By definition, this has identity cost-to-come.
124  startVertex->setCostToComeFromStart(objective_->identityCost());
125 
126  // Add the start vertex to the set of vertices.
127  vertices_.add(startVertex);
128 
129  // Remember it as a start vertex.
130  startVertices_.emplace_back(startVertex);
131  }
132 
133  void ImplicitGraph::registerGoalState(const ompl::base::State *const goalState)
134  {
135  // Create a vertex corresponding to this state.
136  auto goalVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
137 
138  // Copy the state into the vertex's state.
139  spaceInformation_->copyState(goalVertex->getState(), goalState);
140 
141  // Add the goal vertex to the set of vertices.
142  vertices_.add(goalVertex);
143 
144  // Remember it as a goal vertex.
145  goalVertices_.emplace_back(goalVertex);
146  }
147 
148  bool ImplicitGraph::hasAStartState() const
149  {
150  return !startVertices_.empty();
151  }
152 
153  bool ImplicitGraph::hasAGoalState() const
154  {
155  return !goalVertices_.empty();
156  }
157 
158  void
160  ompl::base::PlannerInputStates *inputStates)
161  {
162  // We need to keep track whether a new goal and/or a new start has been added.
163  bool addedNewGoalState = false;
164  bool addedNewStartState = false;
165 
166  // First update the goals. We have to call inputStates->nextGoal(terminationCondition) at least once
167  // (regardless of the return value of inputStates->moreGoalStates()) in case the termination condition
168  // wants us to wait for a goal.
169  do
170  {
171  // Get a new goal. If there are none, or the underlying state is invalid this will be a nullptr.
172  auto newGoalState = inputStates->nextGoal(terminationCondition);
173 
174  // If there was a new valid goal, register it as such and remember that a goal has been added.
175  if (static_cast<bool>(newGoalState))
176  {
177  registerGoalState(newGoalState);
178  addedNewGoalState = true;
179  }
180 
181  } while (inputStates->haveMoreGoalStates() && goalVertices_.size() <= maxNumGoals_);
182 
183  // Having updated the goals, we now update the starts.
184  while (inputStates->haveMoreStartStates())
185  {
186  // Get the next start. The returned pointer can be a nullptr (if the state is invalid).
187  auto newStartState = inputStates->nextStart();
188 
189  // If there is a new valid start, register it as such and remember that a start has been added.
190  if (static_cast<bool>(newStartState))
191  {
192  registerStartState(newStartState);
193  addedNewStartState = true;
194  }
195  }
196 
197  // If we added a new start and have previously pruned goals, we might want to add the goals back.
198  if (addedNewStartState && !prunedGoalVertices_.empty())
199  {
200  // Keep track of the pruned goal vertices that have been revived.
201  std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedGoals;
202 
203  // Let's see if the pruned goal is close enough to any new start to revive it..
204  for (auto it = prunedGoalVertices_.begin(); it != prunedGoalVertices_.end(); ++it)
205  {
206  // Loop over all start states to get the best cost.
207  auto heuristicCost = objective_->infiniteCost();
208  for (const auto &start : startVertices_)
209  {
210  heuristicCost = objective_->betterCost(
211  heuristicCost, objective_->motionCostHeuristic(start->getState(), (*it)->getState()));
212  }
213 
214  // If this goal can possibly improve the current solution, add it back to the graph.
215  if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
216  {
217  registerGoalState((*it)->getState());
218  addedNewGoalState = true;
219  revivedGoals.emplace_back(it);
220  }
221  }
222 
223  // Remove all revived goals from the pruned goals.
224  for (const auto &revivedGoal : revivedGoals)
225  {
226  std::iter_swap(revivedGoal, prunedGoalVertices_.rbegin());
227  prunedGoalVertices_.pop_back();
228  }
229  }
230 
231  // If we added a new goal and have previously pruned starts, we might want to add the starts back.
232  if (addedNewGoalState && !prunedStartVertices_.empty())
233  {
234  // Keep track of the pruned goal vertices that have been revived.
235  std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedStarts;
236 
237  // Let's see if the pruned start is close enough to any new goal to revive it..
238  for (auto it = prunedStartVertices_.begin(); it != prunedStartVertices_.end(); ++it)
239  {
240  // Loop over all start states to get the best cost.
241  auto heuristicCost = objective_->infiniteCost();
242  for (const auto &goal : goalVertices_)
243  {
244  heuristicCost = objective_->betterCost(
245  heuristicCost, objective_->motionCostHeuristic(goal->getState(), (*it)->getState()));
246  }
247 
248  // If this goal can possibly improve the current solution, add it back to the graph.
249  if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
250  {
251  registerStartState((*it)->getState());
252  addedNewStartState = true;
253  revivedStarts.emplace_back(it);
254  }
255  }
256 
257  // Remove all revived goals from the pruned goals.
258  for (const auto &revivedStart : revivedStarts)
259  {
260  std::iter_swap(revivedStart, prunedStartVertices_.rbegin());
261  prunedStartVertices_.pop_back();
262  }
263  }
264 
265  if (addedNewGoalState || addedNewStartState)
266  {
267  // Allocate a state sampler if we have at least one start and one goal.
268  if (!startVertices_.empty() && !goalVertices_.empty())
269  {
270  sampler_ = objective_->allocInformedStateSampler(problemDefinition_,
271  std::numeric_limits<unsigned int>::max());
272  }
273  }
274 
275  if (!goalVertices_.empty() && startVertices_.empty())
276  {
277  OMPL_WARN("AIT* (ImplicitGraph): The problem has a goal but not a start. AIT* can not find a "
278  "solution since PlannerInputStates provides no method to wait for a valid start state to "
279  "appear.");
280  }
281  }
282 
283  std::size_t ImplicitGraph::computeNumberOfSamplesInInformedSet() const
284  {
285  // Loop over all vertices and count the ones in the informed set.
286  std::size_t numberOfSamplesInInformedSet{0u};
287  for (const auto &vertex : getVertices())
288  {
289  // Get the best cost to come from any start.
290  auto costToCome = objective_->infiniteCost();
291  for (const auto &start : startVertices_)
292  {
293  costToCome = objective_->betterCost(
294  costToCome, objective_->motionCostHeuristic(start->getState(), vertex->getState()));
295  }
296 
297  // Get the best cost to go to any goal.
298  auto costToGo = objective_->infiniteCost();
299  for (const auto &goal : goalVertices_)
300  {
301  costToGo = objective_->betterCost(
302  costToCome, objective_->motionCostHeuristic(vertex->getState(), goal->getState()));
303  }
304 
305  // If this can possibly improve the current solution, it is in the informed set.
306  if (objective_->isCostBetterThan(objective_->combineCosts(costToCome, costToGo), solutionCost_))
307  {
308  ++numberOfSamplesInInformedSet;
309  }
310  }
311 
312  return numberOfSamplesInInformedSet;
313  }
314 
315  bool ImplicitGraph::addSamples(std::size_t numNewSamples,
316  const ompl::base::PlannerTerminationCondition &terminationCondition)
317  {
318  // If there are no states to be added, then there's nothing to do.
319  if (numNewSamples == 0u)
320  {
321  return true;
322  }
323 
324  // Ensure there's enough space for the new samples.
325  newSamples_.reserve(numNewSamples);
326 
327  do
328  {
329  // Create a new vertex.
330  newSamples_.emplace_back(std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_));
331 
332  bool foundValidSample = false;
333  do
334  {
335  // Sample the associated state uniformly within the informed set.
336  sampler_->sampleUniform(newSamples_.back()->getState(), solutionCost_);
337 
338  // Count how many states we've checked.
339  ++numSampledStates_;
340 
341  // Check if the sample is valid.
342  foundValidSample = spaceInformation_->getStateValidityChecker()->isValid(newSamples_.back()->getState());
343  } while (!foundValidSample && !terminationCondition);
344 
345  // The sample can be invalid if the termination condition is met.
346  if (foundValidSample)
347  {
348  // If this state happens to satisfy the goal condition, add it as such.
349  if (problemDefinition_->getGoal()->isSatisfied(newSamples_.back()->getState()))
350  {
351  goalVertices_.emplace_back(newSamples_.back());
352  newSamples_.back()->setCostToComeFromGoal(objective_->identityCost());
353  }
354 
355  ++numValidSamples_;
356  }
357  else
358  {
359  // Remove the invalid sample.
360  newSamples_.pop_back();
361  }
362  } while (newSamples_.size() < numNewSamples && !terminationCondition);
363 
364  if (newSamples_.size() == numNewSamples)
365  {
366  // First get the number of samples inside the informed set.
367  auto numSamplesInInformedSet = computeNumberOfSamplesInInformedSet();
368 
369  if (useKNearest_)
370  {
371  numNeighbors_ = computeNumberOfNeighbors(numSamplesInInformedSet + numNewSamples -
372  startVertices_.size() - goalVertices_.size());
373  }
374  else
375  {
376  radius_ = computeConnectionRadius(numSamplesInInformedSet + numNewSamples -
377  startVertices_.size() - goalVertices_.size());
378  }
379 
380  // Add all new vertices to the nearest neighbor structure.
381  vertices_.add(newSamples_);
382  newSamples_.clear();
383 
384  // Update the batch id.
385  ++batchId_;
386 
387  return true;
388  }
389 
390  return false;
391  }
392 
393  std::size_t ImplicitGraph::getNumVertices() const
394  {
395  return vertices_.size();
396  }
397 
398  double ImplicitGraph::getConnectionRadius() const
399  {
400  return radius_;
401  }
402 
403  std::vector<std::shared_ptr<Vertex>>
404  ImplicitGraph::getNeighbors(const std::shared_ptr<Vertex> &vertex) const
405  {
406  // Return cached neighbors if available.
407  if (vertex->hasCachedNeighbors())
408  {
409  return vertex->getNeighbors();
410  }
411  else
412  {
413  ++numNearestNeighborsCalls_;
414  std::vector<std::shared_ptr<Vertex>> neighbors{};
415  if (useKNearest_)
416  {
417  vertices_.nearestK(vertex, numNeighbors_, neighbors);
418  }
419  else
420  {
421  vertices_.nearestR(vertex, radius_, neighbors);
422  }
423  vertex->cacheNeighbors(neighbors);
424  return neighbors;
425  }
426  }
427 
428  bool ImplicitGraph::isStart(const std::shared_ptr<Vertex> &vertex) const
429  {
430  for (const auto &start : startVertices_)
431  {
432  if (vertex->getId() == start->getId())
433  {
434  return true;
435  }
436  }
437  return false;
438  }
439 
440  bool ImplicitGraph::isGoal(const std::shared_ptr<Vertex> &vertex) const
441  {
442  for (const auto &goal : goalVertices_)
443  {
444  if (vertex->getId() == goal->getId())
445  {
446  return true;
447  }
448  }
449  return false;
450  }
451 
452  const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getStartVertices() const
453  {
454  return startVertices_;
455  }
456 
457  const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getGoalVertices() const
458  {
459  return goalVertices_;
460  }
461 
462  std::vector<std::shared_ptr<Vertex>> ImplicitGraph::getVertices() const
463  {
464  std::vector<std::shared_ptr<Vertex>> vertices;
465  vertices_.list(vertices);
466  return vertices;
467  }
468 
469  void ImplicitGraph::prune()
470  {
471  if (!objective_->isFinite(solutionCost_))
472  {
473  return;
474  }
475 
476  std::vector<std::shared_ptr<Vertex>> vertices;
477  vertices_.list(vertices);
478 
479  // Prepare the vector of vertices to be pruned.
480  std::vector<std::shared_ptr<Vertex>> verticesToBePruned;
481 
482  // Check each vertex whether it can be pruned.
483  for (const auto &vertex : vertices)
484  {
485  // Check if the combination of the admissible costToCome and costToGo estimates results in a path
486  // that is more expensive than the current solution.
487  if (!canPossiblyImproveSolution(vertex))
488  {
489  // We keep track of pruned start and goal vertices. This is because if the user adds start or
490  // goal states after we have pruned start or goal states, we might want to reconsider pruned
491  // start or goal states.
492  if (isGoal(vertex))
493  {
494  prunedGoalVertices_.emplace_back(vertex);
495  }
496  else if (isStart(vertex))
497  {
498  prunedStartVertices_.emplace_back(vertex);
499  }
500  verticesToBePruned.emplace_back(vertex);
501  }
502  }
503 
504  // Remove all vertices to be pruned.
505  for (const auto &vertex : verticesToBePruned)
506  {
507  // Remove it from both search trees.
508  if (vertex->hasReverseParent())
509  {
510  vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
511  vertex->resetReverseParent();
512  }
513  vertex->invalidateReverseBranch();
514  if (vertex->hasForwardParent())
515  {
516  vertex->getForwardParent()->removeFromForwardChildren(vertex->getId());
517  vertex->resetForwardParent();
518  }
519  vertex->invalidateForwardBranch();
520 
521  // Remove it from the nearest neighbor struct.
522  vertices_.remove(vertex);
523  }
524 
525  // Assert that the forward and reverse queue are empty?
526  }
527 
528  std::size_t ImplicitGraph::getNumberOfSampledStates() const
529  {
530  return numSampledStates_;
531  }
532 
533  std::size_t ImplicitGraph::getNumberOfValidSamples() const
534  {
535  return numValidSamples_;
536  }
537 
538  std::size_t ImplicitGraph::getNumberOfStateCollisionChecks() const
539  {
540  // Each sampled state is checked for collision. Only sampled states are checked for collision (number of
541  // collision checked edges don't count here.)
542  return numSampledStates_;
543  }
544 
545  std::size_t ImplicitGraph::getNumberOfNearestNeighborCalls() const
546  {
547  return numNearestNeighborsCalls_;
548  }
549 
550  double ImplicitGraph::computeConnectionRadius(std::size_t numSamples) const
551  {
552  // Define the dimension as a helper variable.
553  auto dimension = static_cast<double>(spaceInformation_->getStateDimension());
554 
555  // Compute the RRT* factor.
556  return rewireFactor_ *
557  std::pow(2.0 * (1.0 + 1.0 / dimension) *
558  (sampler_->getInformedMeasure(solutionCost_) /
559  unitNBallMeasure(spaceInformation_->getStateDimension())) *
560  (std::log(static_cast<double>(numSamples)) / static_cast<double>(numSamples)),
561  1.0 / dimension);
562 
563  // // Compute the FMT* factor.
564  // return 2.0 * rewireFactor_ *
565  // std::pow((1.0 / dimension) *
566  // (sampler_->getInformedMeasure(*solutionCost_.lock()) /
567  // unitNBallMeasure(spaceInformation_->getStateDimension())) *
568  // (std::log(static_cast<double>(numSamples)) / numSamples),
569  // 1.0 / dimension);
570  }
571 
572  std::size_t ImplicitGraph::computeNumberOfNeighbors(std::size_t numSamples) const
573  {
574  return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numSamples)));
575  }
576 
577  bool ImplicitGraph::canPossiblyImproveSolution(const std::shared_ptr<Vertex> &vertex) const
578  {
579  // Get the preferred start for this vertex.
580  auto bestCostToCome = objective_->infiniteCost();
581  for (const auto &start : startVertices_)
582  {
583  auto costToCome = objective_->motionCostHeuristic(start->getState(), vertex->getState());
584  if (objective_->isCostBetterThan(costToCome, bestCostToCome))
585  {
586  bestCostToCome = costToCome;
587  }
588  }
589 
590  // Check if the combination of the admissible costToCome and costToGo estimates results in a path
591  // that is more expensive than the current solution.
592  return objective_->isCostBetterThan(
593  objective_->combineCosts(
594  bestCostToCome, objective_->costToGo(vertex->getState(), problemDefinition_->getGoal().get())),
595  solutionCost_);
596  }
597 
598  } // namespace aitstar
599 
600  } // namespace geometric
601 
602 } // 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:339
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:141
Definition of an abstract state.
Definition: State.h:113
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:111
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:228
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:265
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:346
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:21