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 */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_COSTHELPER_
38 #define OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_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/bitstar/BITstar.h"
54 // The vertex class
55 #include "ompl/geometric/planners/bitstar/datastructures/Vertex.h"
56 // The graph class
57 #include "ompl/geometric/planners/bitstar/datastructures/ImplicitGraph.h"
58 
59 namespace ompl
60 {
61  namespace geometric
62  {
70  {
71  public:
73  // Public functions:
75  CostHelper() = default;
76 
77  virtual ~CostHelper() = default;
78 
81  {
82  opt_ = opt;
83  graphPtr_ = graph;
84  };
85 
87  inline void clear()
88  {
89  opt_.reset();
90  graphPtr_ = nullptr;
91  };
92 
95  {
96  return opt_;
97  };
98 
100  // Heuristic helper functions
105  {
106  return this->combineCosts(this->costToComeHeuristic(vertex), this->costToGoHeuristic(vertex));
107  };
108 
113  {
114  return this->combineCosts(vertex->getCost(), this->costToGoHeuristic(vertex));
115  };
116 
121  {
122  return this->combineCosts(this->lowerBoundHeuristicToTarget(edgePair),
123  this->costToGoHeuristic(edgePair.second));
124  };
125 
130  {
131  return this->combineCosts(this->currentHeuristicToTarget(edgePair),
132  this->costToGoHeuristic(edgePair.second));
133  };
134 
139  {
140  return this->combineCosts(this->costToComeHeuristic(edgePair.first), this->edgeCostHeuristic(edgePair));
141  };
142 
147  {
148  return this->combineCosts(edgePair.first->getCost(), this->edgeCostHeuristic(edgePair));
149  };
150 
153  {
154  // Variable
155  // The current best cost to the state, initialize to infinity
156  ompl::base::Cost curBest = this->infiniteCost();
157 
158  // Iterate over the vector of starts, finding the minimum estimated cost-to-come to the state
159  for (auto startIter = graphPtr_->startVerticesBeginConst(); startIter != graphPtr_->startVerticesEndConst();
160  ++startIter)
161  {
162  // Update the cost-to-come as the better of the best so far and the new one
163  curBest = this->betterCost(curBest,
164  this->motionCostHeuristic((*startIter)->stateConst(), vertex->stateConst()));
165  }
166 
167  // Return
168  return curBest;
169  };
170 
173  {
174  return this->motionCostHeuristic(edgePair.first->stateConst(), edgePair.second->stateConst());
175  };
176 
179  {
180  // Variable
181  // The current best cost to a goal from the state, initialize to infinity
182  ompl::base::Cost curBest = this->infiniteCost();
183 
184  // Iterate over the vector of goals, finding the minimum estimated cost-to-go from the state
185  for (auto goalIter = graphPtr_->goalVerticesBeginConst(); goalIter != graphPtr_->goalVerticesEndConst();
186  ++goalIter)
187  {
188  // Update the cost-to-go as the better of the best so far and the new one
189  curBest = this->betterCost(curBest,
190  this->motionCostHeuristic(vertex->stateConst(), (*goalIter)->stateConst()));
191  }
192 
193  // Return
194  return curBest;
195  };
197 
199  // Cost-calculation functions
201  inline ompl::base::Cost trueEdgeCost(const VertexConstPtrPair &edgePair) const
202  {
203  return this->motionCost(edgePair.first->stateConst(), edgePair.second->stateConst());
204  };
205 
208  const ompl::base::Cost &c) const
209  {
210  return this->combineCosts(a, this->combineCosts(b, c));
211  };
212 
215  const ompl::base::Cost &c, const ompl::base::Cost &d) const
216  {
217  return this->combineCosts(a, this->combineCosts(b, c, d));
218  };
220 
222  // Cost-comparison functions
224  inline bool isCostWorseThan(const ompl::base::Cost &a, const ompl::base::Cost &b) const
225  {
226  // If b is better than a, then a is worse than b
227  return this->isCostBetterThan(b, a);
228  };
229 
232  inline bool isCostNotEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
233  {
234  // If a is better than b, or b is better than a, then they are not equal
235  return this->isCostBetterThan(a, b) || this->isCostBetterThan(b, a);
236  };
237 
241  {
242  // If b is not better than a, then a is better than, or equal to, b
243  return !this->isCostBetterThan(b, a);
244  };
245 
249  {
250  // If a is not better than b, than a is worse than, or equal to, b
251  return !this->isCostBetterThan(a, b);
252  };
253 
256  inline double fractionalChange(const ompl::base::Cost &newCost, const ompl::base::Cost &oldCost) const
257  {
258  return this->fractionalChange(newCost, oldCost, oldCost);
259  };
260 
263  inline double fractionalChange(const ompl::base::Cost &newCost, const ompl::base::Cost &oldCost,
264  const ompl::base::Cost &refCost) const
265  {
266  // If the old cost is not finite, than we call that infinite percent improvement
267  if (this->isFinite(oldCost) == false)
268  {
269  // Return infinity (but not beyond)
270  return std::numeric_limits<double>::infinity();
271  }
272  // Calculate and return
273  return (newCost.value() - oldCost.value()) / refCost.value();
274  };
276 
278  // Straight pass-throughs to OptimizationObjective
279  inline bool isSatisfied(const ompl::base::Cost &a) const
280  {
281  return opt_->isSatisfied(a);
282  };
283  inline bool isFinite(const ompl::base::Cost &a) const
284  {
285  return opt_->isFinite(a);
286  };
287  inline bool isCostEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
288  {
289  return opt_->isCostEquivalentTo(a, b);
290  };
291  inline bool isCostBetterThan(const ompl::base::Cost &a, const ompl::base::Cost &b) const
292  {
293  return opt_->isCostBetterThan(a, b);
294  };
295  inline ompl::base::Cost betterCost(const ompl::base::Cost &a, const ompl::base::Cost &b) const
296  {
297  return opt_->betterCost(a, b);
298  };
299  inline ompl::base::Cost combineCosts(const ompl::base::Cost &a, const ompl::base::Cost &b) const
300  {
301  return opt_->combineCosts(a, b);
302  };
303  inline ompl::base::Cost infiniteCost() const
304  {
305  return opt_->infiniteCost();
306  };
307  inline ompl::base::Cost identityCost() const
308  {
309  return opt_->identityCost();
310  };
311  inline ompl::base::Cost motionCostHeuristic(const ompl::base::State *a, const ompl::base::State *b) const
312  {
313  return opt_->motionCostHeuristic(a, b);
314  };
315  inline ompl::base::Cost motionCost(const ompl::base::State *a, const ompl::base::State *b) const
316  {
317  return opt_->motionCost(a, b);
318  };
321 
322  private:
324  // Member variables:
327 
330  ImplicitGraph *graphPtr_;
332  }; // class CostHelper
333  } // geometric
334 } // ompl
335 #endif // OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_COSTHELPER_
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:129
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:240
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:146
ompl::base::Cost combineCosts(const ompl::base::Cost &a, const ompl::base::Cost &b, const ompl::base::Cost &c) const
Combine 3 costs.
Definition: CostHelper.h:207
CostHelper()=default
Construct the heuristic helper, must be setup before use.
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:232
ompl::base::Cost costToComeHeuristic(const VertexConstPtr &vertex) const
Calculate a heuristic estimate of the cost-to-come for a Vertex.
Definition: CostHelper.h:152
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:69
ompl::base::Cost combineCosts(const ompl::base::Cost &a, const ompl::base::Cost &b, const ompl::base::Cost &c, const ompl::base::Cost &d) const
Combine 4 costs.
Definition: CostHelper.h:214
ompl::base::Cost edgeCostHeuristic(const VertexConstPtrPair &edgePair) const
Calculate a heuristic estimate of the cost of an edge between two Vertices.
Definition: CostHelper.h:172
void setup(const ompl::base::OptimizationObjectivePtr &opt, ImplicitGraph *graph)
Setup the CostHelper, must be called before use.
Definition: CostHelper.h:80
VertexPtrVector::const_iterator startVerticesBeginConst() const
Returns a const-iterator to the front of the start-vertex vector.
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:224
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
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:120
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
VertexPtrVector::const_iterator startVerticesEndConst() const
Returns a const-iterator to the end of the start-vertex vector.
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:248
double fractionalChange(const ompl::base::Cost &newCost, const ompl::base::Cost &oldCost, const ompl::base::Cost &refCost) const
Calculate the fractional change of cost "newCost" from "oldCost" relative to "refCost", i.e., (newCost - oldCost)/refCost.
Definition: CostHelper.h:263
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition: BITstar.h:138
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:112
Definition of an abstract state.
Definition: State.h:49
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", i.e., (newCost - oldCost)/oldCost.
Definition: CostHelper.h:256
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:104
A shared pointer wrapper for ompl::base::OptimizationObjective.
A conceptual representation of samples as an edge-implicit random geometric graph.
Definition: ImplicitGraph.h:64
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:138
ompl::base::OptimizationObjectivePtr getOptObj() const
Get the underling OptimizationObjective.
Definition: CostHelper.h:94
VertexPtrVector::const_iterator goalVerticesBeginConst() const
Returns a const-iterator to the front of the goal-vertex vector.
ompl::base::Cost costToGoHeuristic(const VertexConstPtr &vertex) const
Calculate a heuristic estimate of the cost-to-go for a Vertex.
Definition: CostHelper.h:178
double value() const
The value of the cost.
Definition: Cost.h:56
VertexPtrVector::const_iterator goalVerticesEndConst() const
Returns a const-iterator to the end of the goal-vertex vector.
ompl::base::Cost trueEdgeCost(const VertexConstPtrPair &edgePair) const
The true cost of an edge, including constraints.
Definition: CostHelper.h:201
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void clear()
Clear the CostHelper, returns to state at construction.
Definition: CostHelper.h:87