CostHelper.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Toronto
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 University of Toronto 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: Jonathan Gammell, Marlin Strub */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_COSTHELPER_
38 #define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_COSTHELPER_
39 
40 // OMPL:
41 // The cost class:
42 #include "ompl/base/Cost.h"
43 // The optimization objective class:
44 #include "ompl/base/OptimizationObjective.h"
45 // For exceptions:
46 #include "ompl/util/Exception.h"
47 
48 
49 // BIT*:
50 // I am member class of the BITstar class (i.e., I am in it's namespace), so I need to include it's definition to be
51 // aware of the class BITstar. It has a forward declaration to me and the other helper classes but I will need to
52 // include any I use in my .cpp (to avoid dependency loops).
53 #include "ompl/geometric/planners/informedtrees/BITstar.h"
54 // The vertex class
55 #include "ompl/geometric/planners/informedtrees/bitstar/Vertex.h"
56 // The graph class
57 #include "ompl/geometric/planners/informedtrees/bitstar/ImplicitGraph.h"
58 
59 namespace ompl
60 {
61  namespace geometric
62  {
69  class BITstar::CostHelper
70  {
71  public:
73  // Public functions:
75  CostHelper() = default;
76 
77  virtual ~CostHelper() = default;
78 
80  inline void setup(const ompl::base::OptimizationObjectivePtr &opt, ImplicitGraph *graph)
81  {
82  opt_ = opt;
83  graphPtr_ = graph;
84  };
85 
87  inline void reset()
88  {
89  opt_.reset();
90  graphPtr_ = nullptr;
91  };
92 
95  {
96  return opt_;
97  };
98 
100  // Heuristic helper functions
104  inline ompl::base::Cost lowerBoundHeuristicVertex(const VertexConstPtr &vertex) const
105  {
106 #ifdef BITSTAR_DEBUG
107  if (vertex->isPruned())
108  {
109  throw ompl::Exception("Computing the lower bound heuristic through a pruned vertex.");
110  }
111 #endif // BITSTAR_DEBUG
112  return this->combineCosts(this->costToComeHeuristic(vertex), this->costToGoHeuristic(vertex));
113  };
114 
118  inline ompl::base::Cost currentHeuristicVertex(const VertexConstPtr &vertex) const
119  {
120  return this->combineCosts(vertex->getCost(), this->costToGoHeuristic(vertex));
121  };
122 
126  inline ompl::base::Cost lowerBoundHeuristicEdge(const VertexConstPtrPair &edgePair) const
127  {
128  return this->combineCosts(this->lowerBoundHeuristicToTarget(edgePair),
129  this->costToGoHeuristic(edgePair.second));
130  };
131 
135  inline ompl::base::Cost currentHeuristicEdge(const VertexConstPtrPair &edgePair) const
136  {
137  return this->combineCosts(this->currentHeuristicToTarget(edgePair),
138  this->costToGoHeuristic(edgePair.second));
139  };
140 
145  {
146  return this->combineCosts(this->costToComeHeuristic(edgePair.first), this->edgeCostHeuristic(edgePair));
147  };
148 
152  inline ompl::base::Cost currentHeuristicToTarget(const VertexConstPtrPair &edgePair) const
153  {
154  return this->combineCosts(edgePair.first->getCost(), this->edgeCostHeuristic(edgePair));
155  };
156 
158  inline ompl::base::Cost costToComeHeuristic(const VertexConstPtr &vertex) const
159  {
160 #ifdef BITSTAR_DEBUG
161  if (vertex->isPruned())
162  {
163  throw ompl::Exception("Computing the cost to come heuristic to a pruned vertex.");
164  }
165 #endif // BITSTAR_DEBUG
166  // Variable
167  // The current best cost to the state, initialize to infinity
168  ompl::base::Cost curBest = this->infiniteCost();
169 
170  // Iterate over the vector of starts, finding the minimum estimated cost-to-come to the state
171  for (auto startIter = graphPtr_->startVerticesBeginConst(); startIter != graphPtr_->startVerticesEndConst();
172  ++startIter)
173  {
174  // Update the cost-to-come as the better of the best so far and the new one
175  curBest = this->betterCost(curBest,
176  this->motionCostHeuristic((*startIter)->state(), vertex->state()));
177  }
178 
179  // Return
180  return curBest;
181  };
182 
184  inline ompl::base::Cost edgeCostHeuristic(const VertexConstPtrPair &edgePair) const
185  {
186  return this->motionCostHeuristic(edgePair.first->state(), edgePair.second->state());
187  };
188 
191  {
192  // Variable
193  // The current best cost to a goal from the state, initialize to infinity
194  ompl::base::Cost curBest = this->infiniteCost();
195 
196  // Iterate over the vector of goals, finding the minimum estimated cost-to-go from the state
197  for (auto goalIter = graphPtr_->goalVerticesBeginConst(); goalIter != graphPtr_->goalVerticesEndConst();
198  ++goalIter)
199  {
200  // Update the cost-to-go as the better of the best so far and the new one
201  curBest = this->betterCost(curBest,
202  this->motionCostHeuristic(vertex->state(), (*goalIter)->state()));
203  }
204 
205  // Return
206  return curBest;
207  };
209 
211  // Cost-calculation functions
213  inline ompl::base::Cost trueEdgeCost(const VertexConstPtrPair &edgePair) const
214  {
215  return this->motionCost(edgePair.first->state(), edgePair.second->state());
216  };
217 
219  template <typename... Costs>
220  inline ompl::base::Cost combineCosts(const ompl::base::Cost &cost, const Costs&... costs) const
221  {
222  return this->combineCosts(cost, this->combineCosts(costs...));
223  }
224 
226  inline ompl::base::Cost inflateCost(const ompl::base::Cost &cost, double factor) const
227  {
228  return ompl::base::Cost(factor * cost.value());
229  }
230 
232  // Cost-comparison functions
234  inline bool isCostWorseThan(const ompl::base::Cost &a, const ompl::base::Cost &b) const
235  {
236  // If b is better than a, then a is worse than b
237  return this->isCostBetterThan(b, a);
238  };
239 
242  inline bool isCostNotEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
243  {
244  // If a is better than b, or b is better than a, then they are not equal
245  return this->isCostBetterThan(a, b) || this->isCostBetterThan(b, a);
246  };
247 
250  inline bool isCostBetterThanOrEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
251  {
252  // If b is not better than a, then a is better than, or equal to, b
253  return !this->isCostBetterThan(b, a);
254  };
255 
258  inline bool isCostWorseThanOrEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
259  {
260  // If a is not better than b, than a is worse than, or equal to, b
261  return !this->isCostBetterThan(a, b);
262  };
263 
266  inline double fractionalChange(const ompl::base::Cost &newCost, const ompl::base::Cost &oldCost) const
267  {
268  return this->fractionalChange(newCost, oldCost, oldCost);
269  };
270 
273  inline double fractionalChange(const ompl::base::Cost &newCost, const ompl::base::Cost &oldCost,
274  const ompl::base::Cost &refCost) const
275  {
276  // If the old cost is not finite, than we call that infinite percent improvement
277  if (!this->isFinite(oldCost))
278  {
279  // Return infinity (but not beyond)
280  return std::numeric_limits<double>::infinity();
281  }
282  // Calculate and return
283  return (newCost.value() - oldCost.value()) / refCost.value();
284  };
286 
288  // Straight pass-throughs to OptimizationObjective
289  inline bool isSatisfied(const ompl::base::Cost &a) const
290  {
291  return opt_->isSatisfied(a);
292  };
293  inline bool isFinite(const ompl::base::Cost &a) const
294  {
295  return opt_->isFinite(a);
296  };
297  inline bool isCostEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
298  {
299  return opt_->isCostEquivalentTo(a, b);
300  };
301  inline bool isCostBetterThan(const ompl::base::Cost &a, const ompl::base::Cost &b) const
302  {
303  return opt_->isCostBetterThan(a, b);
304  };
305  inline ompl::base::Cost betterCost(const ompl::base::Cost &a, const ompl::base::Cost &b) const
306  {
307  return opt_->betterCost(a, b);
308  };
310  {
311  return opt_->combineCosts(a, b);
312  };
313  inline ompl::base::Cost infiniteCost() const
314  {
315  return opt_->infiniteCost();
316  };
317  inline ompl::base::Cost identityCost() const
318  {
319  return opt_->identityCost();
320  };
321  inline ompl::base::Cost motionCostHeuristic(const ompl::base::State *a, const ompl::base::State *b) const
322  {
323  return opt_->motionCostHeuristic(a, b);
324  };
325  inline ompl::base::Cost motionCost(const ompl::base::State *a, const ompl::base::State *b) const
326  {
327  return opt_->motionCost(a, b);
328  };
331 
332  private:
334  // Member variables:
337 
340  ImplicitGraph *graphPtr_;
342  }; // class CostHelper
343  } // geometric
344 } // ompl
345 #endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_COSTHELPER_
VertexPtrVector::const_iterator goalVerticesBeginConst() const
Returns a const-iterator to the front of the goal-vertex vector.
VertexPtrVector::const_iterator goalVerticesEndConst() const
Returns a const-iterator to the end of the goal-vertex vector.
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:215
ompl::base::Cost lowerBoundHeuristicVertex(const VertexConstPtr &vertex) const
Calculates a heuristic estimate of the cost of a solution constrained to pass through a vertex,...
Definition: CostHelper.h:200
ompl::base::Cost lowerBoundHeuristicToTarget(const VertexConstPtrPair &edgePair) const
Calculates a heuristic estimate of the cost of a path to the target of an edge, independent of the cu...
Definition: CostHelper.h:240
Definition of an abstract state.
Definition: State.h:113
CostHelper()=default
Construct the heuristic helper, must be setup before use.
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition: BITstar.h:233
ompl::base::Cost currentHeuristicEdge(const VertexConstPtrPair &edgePair) const
Calculates a heuristic estimate of the cost of a solution constrained to go through an edge,...
Definition: CostHelper.h:231
ompl::base::Cost edgeCostHeuristic(const VertexConstPtrPair &edgePair) const
Calculate a heuristic estimate of the cost of an edge between two Vertices.
Definition: CostHelper.h:280
ompl::base::Cost costToComeHeuristic(const VertexConstPtr &vertex) const
Calculate a heuristic estimate of the cost-to-come for a Vertex.
Definition: CostHelper.h:254
ompl::base::OptimizationObjectivePtr getOptObj() const
Get the underling OptimizationObjective.
Definition: CostHelper.h:190
ompl::base::Cost lowerBoundHeuristicEdge(const VertexConstPtrPair &edgePair) const
Calculates a heuristic estimate of the cost of a solution constrained to go through an edge,...
Definition: CostHelper.h:222
bool isCostNotEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
Compare whether cost a and cost b are not equivalent by checking if either a or b is better than the ...
Definition: CostHelper.h:338
double value() const
The value of the cost.
Definition: Cost.h:152
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
A shared pointer wrapper for ompl::base::OptimizationObjective.
VertexPtrVector::const_iterator startVerticesEndConst() const
Returns a const-iterator to the end of the start-vertex vector.
ompl::base::Cost currentHeuristicToTarget(const VertexConstPtrPair &edgePair) const
Calculates a heuristic estimate of the cost of a path to the target of an edge, dependent on the cost...
Definition: CostHelper.h:248
ompl::base::Cost combineCosts(const ompl::base::Cost &cost, const Costs &... costs) const
Combine multiple costs.
Definition: CostHelper.h:316
ompl::base::Cost currentHeuristicVertex(const VertexConstPtr &vertex) const
Calculates a heuristic estimate of the cost of a solution constrained to pass through a vertex,...
Definition: CostHelper.h:214
void setup(const ompl::base::OptimizationObjectivePtr &opt, ImplicitGraph *graph)
Setup the CostHelper, must be called before use.
Definition: CostHelper.h:176
bool isCostBetterThanOrEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
Compare whether cost a is better or equivalent to cost b by checking that b is not better than a.
Definition: CostHelper.h:346
double fractionalChange(const ompl::base::Cost &newCost, const ompl::base::Cost &oldCost) const
Calculate the fractional change of cost "newCost" from "oldCost" relative to "oldCost",...
Definition: CostHelper.h:362
ompl::base::Cost costToGoHeuristic(const VertexConstPtr &vertex) const
Calculate a heuristic estimate of the cost-to-go for a Vertex.
Definition: CostHelper.h:286
bool isCostWorseThan(const ompl::base::Cost &a, const ompl::base::Cost &b) const
Compare whether cost a is worse than cost b by checking whether b is better than a.
Definition: CostHelper.h:330
ompl::base::Cost trueEdgeCost(const VertexConstPtrPair &edgePair) const
The true cost of an edge, including constraints.
Definition: CostHelper.h:309
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
ompl::base::Cost inflateCost(const ompl::base::Cost &cost, double factor) const
Inflate a cost by a given factor.
Definition: CostHelper.h:322
void reset()
Reset the CostHelper, returns to state at construction.
Definition: CostHelper.h:183
The exception type for ompl.
Definition: Exception.h:78
bool isCostWorseThanOrEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
Compare whether cost a is worse or equivalent to cost b by checking that a is not better than b.
Definition: CostHelper.h:354
Main namespace. Contains everything in this library.
Definition: AppBase.h:21