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 
46 // BIT*:
47 // 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
48 // aware of the class BITstar. It has a forward declaration to me and the other helper classes but I will need to
49 // include any I use in my .cpp (to avoid dependency loops).
50 #include "ompl/geometric/planners/bitstar/BITstar.h"
51 // Include the vertex class here so we can inline a bunch of functions:
52 #include "ompl/geometric/planners/bitstar/datastructures/Vertex.h"
53 
54 namespace ompl
55 {
56  namespace geometric
57  {
65  {
66  public:
68  // Public functions:
70  CostHelper() = default;
71 
72  virtual ~CostHelper() = default;
73 
75  void setup(const ompl::base::OptimizationObjectivePtr &opt, const ImplicitGraphPtr &graph);
76 
78  void clear();
79 
82  {
83  return opt_;
84  };
85 
87  // Heuristic helper functions
92  {
93  return this->combineCosts(this->costToComeHeuristic(vertex), this->costToGoHeuristic(vertex));
94  };
95 
100  {
101  return this->combineCosts(vertex->getCost(), this->costToGoHeuristic(vertex));
102  };
103 
108  {
109  return this->combineCosts(this->lowerBoundHeuristicTarget(edgePair),
110  this->costToGoHeuristic(edgePair.second));
111  };
112 
117  {
118  return this->combineCosts(this->currentHeuristicTarget(edgePair),
119  this->costToGoHeuristic(edgePair.second));
120  };
121 
126  {
127  return this->combineCosts(this->costToComeHeuristic(edgePair.first), this->edgeCostHeuristic(edgePair));
128  };
129 
134  {
135  return this->combineCosts(edgePair.first->getCost(), this->edgeCostHeuristic(edgePair));
136  };
137 
140 
143  {
144  return this->motionCostHeuristic(edgePair.first->stateConst(), edgePair.second->stateConst());
145  };
146 
150 
152  // Cost-calculation functions
154  inline ompl::base::Cost trueEdgeCost(const VertexConstPtrPair &edgePair) const
155  {
156  return this->motionCost(edgePair.first->stateConst(), edgePair.second->stateConst());
157  };
158 
161  const ompl::base::Cost &c) const
162  {
163  return this->combineCosts(a, this->combineCosts(b, c));
164  };
165 
168  const ompl::base::Cost &c, const ompl::base::Cost &d) const
169  {
170  return this->combineCosts(a, this->combineCosts(b, c, d));
171  };
173 
175  // Cost-comparison functions
177  inline bool isCostWorseThan(const ompl::base::Cost &a, const ompl::base::Cost &b) const
178  {
179  // If b is better than a, then a is worse than b
180  return this->isCostBetterThan(b, a);
181  };
182 
185  inline bool isCostNotEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
186  {
187  // If a is better than b, or b is better than a, then they are not equal
188  return this->isCostBetterThan(a, b) || this->isCostBetterThan(b, a);
189  };
190 
194  {
195  // If b is not better than a, then a is better than, or equal to, b
196  return !this->isCostBetterThan(b, a);
197  };
198 
202  {
203  // If a is not better than b, than a is worse than, or equal to, b
204  return !this->isCostBetterThan(a, b);
205  };
206 
209  inline double fractionalChange(const ompl::base::Cost &newCost, const ompl::base::Cost &oldCost) const
210  {
211  return this->fractionalChange(newCost, oldCost, oldCost);
212  };
213 
216  inline double fractionalChange(const ompl::base::Cost &newCost, const ompl::base::Cost &oldCost,
217  const ompl::base::Cost &refCost) const
218  {
219  // If the old cost is not finite, than we call that infinite percent improvement
220  if (this->isFinite(oldCost) == false)
221  {
222  // Return infinity (but not beyond)
223  return std::numeric_limits<double>::infinity();
224  }
225  // Calculate and return
226  return (newCost.value() - oldCost.value()) / refCost.value();
227  };
229 
231  // Straight pass-throughs to OptimizationObjective
232  inline bool isSatisfied(const ompl::base::Cost &a) const
233  {
234  return opt_->isSatisfied(a);
235  };
236 
237  inline bool isFinite(const ompl::base::Cost &a) const
238  {
239  return opt_->isFinite(a);
240  };
241  inline bool isCostEquivalentTo(const ompl::base::Cost &a, const ompl::base::Cost &b) const
242  {
243  return opt_->isCostEquivalentTo(a, b);
244  };
245  inline bool isCostBetterThan(const ompl::base::Cost &a, const ompl::base::Cost &b) const
246  {
247  return opt_->isCostBetterThan(a, b);
248  };
249  inline ompl::base::Cost betterCost(const ompl::base::Cost &a, const ompl::base::Cost &b) const
250  {
251  return opt_->betterCost(a, b);
252  };
253  inline ompl::base::Cost combineCosts(const ompl::base::Cost &a, const ompl::base::Cost &b) const
254  {
255  return opt_->combineCosts(a, b);
256  };
257  inline ompl::base::Cost infiniteCost() const
258  {
259  return opt_->infiniteCost();
260  };
261  inline ompl::base::Cost identityCost() const
262  {
263  return opt_->identityCost();
264  };
265  inline ompl::base::Cost motionCostHeuristic(const ompl::base::State *a, const ompl::base::State *b) const
266  {
267  return opt_->motionCostHeuristic(a, b);
268  };
269  inline ompl::base::Cost motionCost(const ompl::base::State *a, const ompl::base::State *b) const
270  {
271  return opt_->motionCost(a, b);
272  };
275 
276  private:
278  // Member variables:
281 
284  ImplicitGraphPtr graphPtr_;
286  }; // class CostHelper
287  } // geometric
288 } // ompl
289 #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:116
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:193
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:160
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:185
ompl::base::Cost costToComeHeuristic(const VertexConstPtr &vertex) const
Calculate a heuristic estimate of the cost-to-come for a Vertex.
Definition: CostHelper.cpp:80
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:64
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:167
ompl::base::Cost lowerBoundHeuristicTarget(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:125
ompl::base::Cost edgeCostHeuristic(const VertexConstPtrPair &edgePair) const
Calculate a heuristic estimate of the cost an edge between two Vertices.
Definition: CostHelper.h:142
std::shared_ptr< ImplicitGraph > ImplicitGraphPtr
An implicit graph shared pointer.
Definition: BITstar.h:148
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:177
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:107
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
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:201
ompl::base::Cost currentHeuristicTarget(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:133
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:216
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:99
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:209
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:91
A shared pointer wrapper for ompl::base::OptimizationObjective.
void setup(const ompl::base::OptimizationObjectivePtr &opt, const ImplicitGraphPtr &graph)
Setup the CostHelper, must be called before use.
Definition: CostHelper.cpp:48
ompl::base::OptimizationObjectivePtr getOptObj() const
Get the underling OptimizationObjective.
Definition: CostHelper.h:81
ompl::base::Cost costToGoHeuristic(const VertexConstPtr &vertex) const
Calculate a heuristic estimate of the cost-to-go for a Vertex.
Definition: CostHelper.cpp:61
double value() const
The value of the cost.
Definition: Cost.h:56
ompl::base::Cost trueEdgeCost(const VertexConstPtrPair &edgePair) const
The true cost of an edge, including collisions.
Definition: CostHelper.h:154
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.cpp:55