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  do
333  {
334  // Sample the associated state uniformly within the informed set.
335  sampler_->sampleUniform(newSamples_.back()->getState(), solutionCost_);
336 
337  // Count how many states we've checked.
338  ++numSampledStates_;
339  } while (!spaceInformation_->getStateValidityChecker()->isValid(newSamples_.back()->getState()));
340 
341  // If this state happens to satisfy the goal condition, add it as such.
342  if (problemDefinition_->getGoal()->isSatisfied(newSamples_.back()->getState()))
343  {
344  goalVertices_.emplace_back(newSamples_.back());
345  newSamples_.back()->setCostToComeFromGoal(objective_->identityCost());
346  }
347 
348  ++numValidSamples_;
349  } while (newSamples_.size() < numNewSamples && !terminationCondition);
350 
351  if (newSamples_.size() == numNewSamples)
352  {
353  // First get the number of samples inside the informed set.
354  auto numSamplesInInformedSet = computeNumberOfSamplesInInformedSet();
355 
356  if (useKNearest_)
357  {
358  numNeighbors_ = computeNumberOfNeighbors(numSamplesInInformedSet + numNewSamples -
359  startVertices_.size() - goalVertices_.size());
360  }
361  else
362  {
363  radius_ = computeConnectionRadius(numSamplesInInformedSet + numNewSamples -
364  startVertices_.size() - goalVertices_.size());
365  }
366 
367  // Add all new vertices to the nearest neighbor structure.
368  vertices_.add(newSamples_);
369  newSamples_.clear();
370 
371  // Update the batch id.
372  ++batchId_;
373 
374  return true;
375  }
376 
377  return false;
378  }
379 
380  std::size_t ImplicitGraph::getNumVertices() const
381  {
382  return vertices_.size();
383  }
384 
385  double ImplicitGraph::getConnectionRadius() const
386  {
387  return radius_;
388  }
389 
390  std::vector<std::shared_ptr<Vertex>>
391  ImplicitGraph::getNeighbors(const std::shared_ptr<Vertex> &vertex) const
392  {
393  // Return cached neighbors if available.
394  if (vertex->hasCachedNeighbors())
395  {
396  return vertex->getNeighbors();
397  }
398  else
399  {
400  ++numNearestNeighborsCalls_;
401  std::vector<std::shared_ptr<Vertex>> neighbors{};
402  if (useKNearest_)
403  {
404  vertices_.nearestK(vertex, numNeighbors_, neighbors);
405  }
406  else
407  {
408  vertices_.nearestR(vertex, radius_, neighbors);
409  }
410  vertex->cacheNeighbors(neighbors);
411  return neighbors;
412  }
413  }
414 
415  bool ImplicitGraph::isStart(const std::shared_ptr<Vertex> &vertex) const
416  {
417  for (const auto &start : startVertices_)
418  {
419  if (vertex->getId() == start->getId())
420  {
421  return true;
422  }
423  }
424  return false;
425  }
426 
427  bool ImplicitGraph::isGoal(const std::shared_ptr<Vertex> &vertex) const
428  {
429  for (const auto &goal : goalVertices_)
430  {
431  if (vertex->getId() == goal->getId())
432  {
433  return true;
434  }
435  }
436  return false;
437  }
438 
439  const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getStartVertices() const
440  {
441  return startVertices_;
442  }
443 
444  const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getGoalVertices() const
445  {
446  return goalVertices_;
447  }
448 
449  std::vector<std::shared_ptr<Vertex>> ImplicitGraph::getVertices() const
450  {
451  std::vector<std::shared_ptr<Vertex>> vertices;
452  vertices_.list(vertices);
453  return vertices;
454  }
455 
456  void ImplicitGraph::prune()
457  {
458  if (!objective_->isFinite(solutionCost_))
459  {
460  return;
461  }
462 
463  std::vector<std::shared_ptr<Vertex>> vertices;
464  vertices_.list(vertices);
465 
466  // Prepare the vector of vertices to be pruned.
467  std::vector<std::shared_ptr<Vertex>> verticesToBePruned;
468 
469  // Check each vertex whether it can be pruned.
470  for (const auto &vertex : vertices)
471  {
472  // Check if the combination of the admissible costToCome and costToGo estimates results in a path
473  // that is more expensive than the current solution.
474  if (!canPossiblyImproveSolution(vertex))
475  {
476  // We keep track of pruned start and goal vertices. This is because if the user adds start or
477  // goal states after we have pruned start or goal states, we might want to reconsider pruned
478  // start or goal states.
479  if (isGoal(vertex))
480  {
481  prunedGoalVertices_.emplace_back(vertex);
482  }
483  else if (isStart(vertex))
484  {
485  prunedStartVertices_.emplace_back(vertex);
486  }
487  verticesToBePruned.emplace_back(vertex);
488  }
489  }
490 
491  // Remove all vertices to be pruned.
492  for (const auto &vertex : verticesToBePruned)
493  {
494  // Remove it from both search trees.
495  if (vertex->hasReverseParent())
496  {
497  vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
498  vertex->resetReverseParent();
499  }
500  vertex->invalidateReverseBranch();
501  if (vertex->hasForwardParent())
502  {
503  vertex->getForwardParent()->removeFromForwardChildren(vertex->getId());
504  vertex->resetForwardParent();
505  }
506  vertex->invalidateForwardBranch();
507 
508  // Remove it from the nearest neighbor struct.
509  vertices_.remove(vertex);
510  }
511 
512  // Assert that the forward and reverse queue are empty?
513  }
514 
515  std::size_t ImplicitGraph::getNumberOfSampledStates() const
516  {
517  return numSampledStates_;
518  }
519 
520  std::size_t ImplicitGraph::getNumberOfValidSamples() const
521  {
522  return numValidSamples_;
523  }
524 
525  std::size_t ImplicitGraph::getNumberOfStateCollisionChecks() const
526  {
527  // Each sampled state is checked for collision. Only sampled states are checked for collision (number of
528  // collision checked edges don't count here.)
529  return numSampledStates_;
530  }
531 
532  std::size_t ImplicitGraph::getNumberOfNearestNeighborCalls() const
533  {
534  return numNearestNeighborsCalls_;
535  }
536 
537  double ImplicitGraph::computeConnectionRadius(std::size_t numSamples) const
538  {
539  // Define the dimension as a helper variable.
540  auto dimension = static_cast<double>(spaceInformation_->getStateDimension());
541 
542  // Compute the RRT* factor.
543  return rewireFactor_ *
544  std::pow(2.0 * (1.0 + 1.0 / dimension) *
545  (sampler_->getInformedMeasure(solutionCost_) /
546  unitNBallMeasure(spaceInformation_->getStateDimension())) *
547  (std::log(static_cast<double>(numSamples)) / static_cast<double>(numSamples)),
548  1.0 / dimension);
549 
550  // // Compute the FMT* factor.
551  // return 2.0 * rewireFactor_ *
552  // std::pow((1.0 / dimension) *
553  // (sampler_->getInformedMeasure(*solutionCost_.lock()) /
554  // unitNBallMeasure(spaceInformation_->getStateDimension())) *
555  // (std::log(static_cast<double>(numSamples)) / numSamples),
556  // 1.0 / dimension);
557  }
558 
559  std::size_t ImplicitGraph::computeNumberOfNeighbors(std::size_t numSamples) const
560  {
561  return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numSamples)));
562  }
563 
564  bool ImplicitGraph::canPossiblyImproveSolution(const std::shared_ptr<Vertex> &vertex) const
565  {
566  // Get the preferred start for this vertex.
567  auto bestCostToCome = objective_->infiniteCost();
568  for (const auto &start : startVertices_)
569  {
570  auto costToCome = objective_->motionCostHeuristic(start->getState(), vertex->getState());
571  if (objective_->isCostBetterThan(costToCome, bestCostToCome))
572  {
573  bestCostToCome = costToCome;
574  }
575  }
576 
577  // Check if the combination of the admissible costToCome and costToGo estimates results in a path
578  // that is more expensive than the current solution.
579  return objective_->isCostBetterThan(
580  objective_->combineCosts(
581  bestCostToCome, objective_->costToGo(vertex->getState(), problemDefinition_->getGoal().get())),
582  solutionCost_);
583  }
584 
585  } // namespace aitstar
586 
587  } // namespace geometric
588 
589 } // 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