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 
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 
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 
91  {
92  return rewireFactor_;
93  }
94 
95  void ImplicitGraph::setUseKNearest(bool useKNearest)
96  {
97  useKNearest_ = useKNearest;
98  }
99 
101  {
102  return useKNearest_;
103  }
104 
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 
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 
139  {
140  return !startVertices_.empty();
141  }
142 
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  bool ImplicitGraph::addSamples(std::size_t numNewSamples,
318  const ompl::base::PlannerTerminationCondition &terminationCondition)
319  {
320  // If there are no states to be added, then there's nothing to do.
321  if (numNewSamples == 0u)
322  {
323  return true;
324  }
325 
326  // Ensure there's enough space for the new samples.
327  newSamples_.reserve(numNewSamples);
328 
329  do
330  {
331  // Create a new vertex.
332  newSamples_.emplace_back(std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_));
333 
334  do
335  {
336  // Sample the associated state uniformly within the informed set.
337  sampler_->sampleUniform(newSamples_.back()->getState(), solutionCost_);
338 
339  // Count how many states we've checked.
340  ++numSampledStates_;
341  } while (!spaceInformation_->getStateValidityChecker()->isValid(newSamples_.back()->getState()));
342 
343  // If this state happens to satisfy the goal condition, add it as such.
344  if (problemDefinition_->getGoal()->isSatisfied(newSamples_.back()->getState()))
345  {
346  goalVertices_.emplace_back(newSamples_.back());
347  }
348 
349  ++numValidSamples_;
350  } while (newSamples_.size() < numNewSamples && !terminationCondition);
351 
352  if (newSamples_.size() == numNewSamples)
353  {
354  // First get the number of samples inside the informed set.
355  auto numSamplesInInformedSet = computeNumberOfSamplesInInformedSet();
356 
357  if (useKNearest_)
358  {
359  numNeighbors_ = computeNumberOfNeighbors(numSamplesInInformedSet + numNewSamples -
360  startVertices_.size() - goalVertices_.size());
361  }
362  else
363  {
364  radius_ = computeConnectionRadius(numSamplesInInformedSet + numNewSamples -
365  startVertices_.size() - goalVertices_.size());
366  }
367 
368  // Add all new vertices to the nearest neighbor structure.
369  vertices_.add(newSamples_);
370  newSamples_.clear();
371 
372  // Update the batch id.
373  ++batchId_;
374 
375  return true;
376  }
377 
378  return false;
379  }
380 
381  std::size_t ImplicitGraph::getNumVertices() const
382  {
383  return vertices_.size();
384  }
385 
387  {
388  return radius_;
389  }
390 
391  std::vector<std::shared_ptr<Vertex>>
392  ImplicitGraph::getNeighbors(const std::shared_ptr<Vertex> &vertex) const
393  {
394  // Return cached neighbors if available.
395  if (vertex->hasCachedNeighbors())
396  {
397  return vertex->getNeighbors();
398  }
399  else
400  {
401  ++numNearestNeighborsCalls_;
402  std::vector<std::shared_ptr<Vertex>> neighbors{};
403  if (useKNearest_)
404  {
405  vertices_.nearestK(vertex, numNeighbors_, neighbors);
406  }
407  else
408  {
409  vertices_.nearestR(vertex, radius_, neighbors);
410  }
411  vertex->cacheNeighbors(neighbors);
412  return neighbors;
413  }
414  }
415 
416  bool ImplicitGraph::isStart(const std::shared_ptr<Vertex> &vertex) const
417  {
418  for (const auto &start : startVertices_)
419  {
420  if (vertex->getId() == start->getId())
421  {
422  return true;
423  }
424  }
425  return false;
426  }
427 
428  bool ImplicitGraph::isGoal(const std::shared_ptr<Vertex> &vertex) const
429  {
430  for (const auto &goal : goalVertices_)
431  {
432  if (vertex->getId() == goal->getId())
433  {
434  return true;
435  }
436  }
437  return false;
438  }
439 
440  const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getStartVertices() const
441  {
442  return startVertices_;
443  }
444 
445  const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getGoalVertices() const
446  {
447  return goalVertices_;
448  }
449 
450  std::vector<std::shared_ptr<Vertex>> ImplicitGraph::getVertices() const
451  {
452  std::vector<std::shared_ptr<Vertex>> vertices;
453  vertices_.list(vertices);
454  return vertices;
455  }
456 
458  {
459  if (!objective_->isFinite(solutionCost_))
460  {
461  return;
462  }
463 
464  std::vector<std::shared_ptr<Vertex>> vertices;
465  vertices_.list(vertices);
466 
467  // Prepare the vector of vertices to be pruned.
468  std::vector<std::shared_ptr<Vertex>> verticesToBePruned;
469 
470  // Check each vertex whether it can be pruned.
471  for (const auto &vertex : vertices)
472  {
473  // Check if the combination of the admissible costToCome and costToGo estimates results in a path
474  // that is more expensive than the current solution.
475  if (!canPossiblyImproveSolution(vertex))
476  {
477  // We keep track of pruned start and goal vertices. This is because if the user adds start or
478  // goal states after we have pruned start or goal states, we might want to reconsider pruned
479  // start or goal states.
480  if (isGoal(vertex))
481  {
482  prunedGoalVertices_.emplace_back(vertex);
483  }
484  else if (isStart(vertex))
485  {
486  prunedStartVertices_.emplace_back(vertex);
487  }
488  verticesToBePruned.emplace_back(vertex);
489  }
490  }
491 
492  // Remove all vertices to be pruned.
493  for (const auto &vertex : verticesToBePruned)
494  {
495  // Remove it from both search trees.
496  if (vertex->hasReverseParent())
497  {
498  vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
499  vertex->resetReverseParent();
500  }
501  vertex->invalidateReverseBranch();
502  if (vertex->hasForwardParent())
503  {
504  vertex->getForwardParent()->removeFromForwardChildren(vertex->getId());
505  vertex->resetForwardParent();
506  }
507  vertex->invalidateForwardBranch();
508 
509  // Remove it from the nearest neighbor struct.
510  vertices_.remove(vertex);
511  }
512 
513  // Assert that the forward and reverse queue are empty?
514  }
515 
517  {
518  return numSampledStates_;
519  }
520 
522  {
523  return numValidSamples_;
524  }
525 
527  {
528  // Each sampled state is checked for collision. Only sampled states are checked for collision (number of
529  // collision checked edges don't count here.)
530  return numSampledStates_;
531  }
532 
534  {
535  return numNearestNeighborsCalls_;
536  }
537 
538  double ImplicitGraph::computeConnectionRadius(std::size_t numSamples) const
539  {
540  // Define the dimension as a helper variable.
541  auto dimension = static_cast<double>(spaceInformation_->getStateDimension());
542 
543  // Compute the RRT* factor.
544  return rewireFactor_ *
545  std::pow(2.0 * (1.0 + 1.0 / dimension) *
546  (sampler_->getInformedMeasure(solutionCost_) /
547  unitNBallMeasure(spaceInformation_->getStateDimension())) *
548  (std::log(static_cast<double>(numSamples)) / static_cast<double>(numSamples)),
549  1.0 / dimension);
550 
551  // // Compute the FMT* factor.
552  // return 2.0 * rewireFactor_ *
553  // std::pow((1.0 / dimension) *
554  // (sampler_->getInformedMeasure(*solutionCost_.lock()) /
555  // unitNBallMeasure(spaceInformation_->getStateDimension())) *
556  // (std::log(static_cast<double>(numSamples)) / numSamples),
557  // 1.0 / dimension);
558  }
559 
560  std::size_t ImplicitGraph::computeNumberOfNeighbors(std::size_t numSamples) const
561  {
562  return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numSamples)));
563  }
564 
565  bool ImplicitGraph::canPossiblyImproveSolution(const std::shared_ptr<Vertex> &vertex) const
566  {
567  // Get the preferred start for this vertex.
568  auto bestCostToCome = objective_->infiniteCost();
569  for (const auto &start : startVertices_)
570  {
571  auto costToCome = objective_->motionCostHeuristic(start->getState(), vertex->getState());
572  if (objective_->isCostBetterThan(costToCome, bestCostToCome))
573  {
574  bestCostToCome = costToCome;
575  }
576  }
577 
578  // Check if the combination of the admissible costToCome and costToGo estimates results in a path
579  // that is more expensive than the current solution.
580  return objective_->isCostBetterThan(
581  objective_->combineCosts(
582  bestCostToCome, objective_->costToGo(vertex->getState(), problemDefinition_->getGoal().get())),
583  solutionCost_);
584  }
585 
586  } // namespace aitstar
587 
588  } // namespace geometric
589 
590 } // namespace ompl
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
bool hasAStartState() const
Returns whether the graph has a goal state.
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
A shared pointer wrapper for ompl::base::SpaceInformation.
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:78
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, ompl::base::PlannerInputStates *inputStates)
The setup method for the graph. Needed to have it on the stack.
std::size_t getNumberOfNearestNeighborCalls() const
Get the number of nearest neighbor calls.
void updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition, ompl::base::PlannerInputStates *inputStates)
Adds new start and goals to the graph if avavilable and creates a new informed sampler if necessary.
bool getUseKNearest() const
Whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.
Definition of an abstract state.
Definition: State.h:50
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
bool hasAGoalState() const
Returns whether the graph has a goal state.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
void registerStartState(const ompl::base::State *const startState)
Registers a state as a start state.
const std::vector< std::shared_ptr< Vertex > > & getStartVertices() const
Get the start vertices.
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.
void registerGoalState(const ompl::base::State *const goalState)
Registers a state as a goal state.
std::size_t getNumberOfValidSamples() const
Returns the total number of valid samples found.
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
void clear()
Resets the graph to its construction state, without resetting options.
double getRewireFactor() const
Get the reqire factor of the RGG.
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
std::size_t getNumberOfSampledStates() const
Returns the total number of sampled states.
A shared pointer wrapper for ompl::base::ProblemDefinition.
double getConnectionRadius() const
Gets the RGG connection radius.
std::size_t getNumVertices() const
Gets the number of samples in the graph.
bool isStart(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a start vertex.
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
void prune()
Prune all samples that can not contribute to a solution better than the current one.
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:355
bool addSamples(std::size_t numNewSamples, const ompl::base::PlannerTerminationCondition &terminationCondition)
Adds a batch of samples and returns the samples it has added.
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG.
bool isGoal(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a goal vertex.
ImplicitGraph(const ompl::base::Cost &solutionCost)
Constructs an implicit graph.
std::size_t getNumberOfStateCollisionChecks() const
Get the number of state collision checks.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22