Loading...
Searching...
No Matches
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
59namespace ompl
60{
61 namespace geometric
62 {
67
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
94 inline ompl::base::OptimizationObjectivePtr getOptObj() const
95 {
96 return opt_;
97 };
98
100 // Heuristic helper functions
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
119 {
120 return this->combineCosts(vertex->getCost(), this->costToGoHeuristic(vertex));
121 };
122
127 {
128 return this->combineCosts(this->lowerBoundHeuristicToTarget(edgePair),
129 this->costToGoHeuristic(edgePair.second));
130 };
131
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
153 {
154 return this->combineCosts(edgePair.first->getCost(), this->edgeCostHeuristic(edgePair));
155 };
156
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
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 };
208
209
211 // Cost-calculation functions
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
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
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 };
285
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 };
309 inline ompl::base::Cost combineCosts(const ompl::base::Cost &a, const ompl::base::Cost &b) const
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:
336 ompl::base::OptimizationObjectivePtr opt_;
337
340 ImplicitGraph *graphPtr_;
342 }; // class CostHelper
343 } // geometric
344} // ompl
345#endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_COSTHELPER_
The exception type for ompl.
Definition Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
double value() const
The value of the cost.
Definition Cost.h:56
A helper class to handle the various heuristic functions in one place.
Definition CostHelper.h:70
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:126
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:118
ompl::base::OptimizationObjectivePtr getOptObj() const
Get the underling OptimizationObjective.
Definition CostHelper.h:94
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:258
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:144
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:250
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:234
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:242
ompl::base::Cost edgeCostHeuristic(const VertexConstPtrPair &edgePair) const
Calculate a heuristic estimate of the cost of an edge between two Vertices.
Definition CostHelper.h:184
CostHelper()=default
Construct the heuristic helper, must be setup before use.
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:152
ompl::base::Cost costToGoHeuristic(const VertexConstPtr &vertex) const
Calculate a heuristic estimate of the cost-to-go for a Vertex.
Definition CostHelper.h:190
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:135
ompl::base::Cost combineCosts(const ompl::base::Cost &cost, const Costs &... costs) const
Combine multiple costs.
Definition CostHelper.h:220
void setup(const ompl::base::OptimizationObjectivePtr &opt, ImplicitGraph *graph)
Setup the CostHelper, must be called before use.
Definition CostHelper.h:80
ompl::base::Cost inflateCost(const ompl::base::Cost &cost, double factor) const
Inflate a cost by a given factor.
Definition CostHelper.h:226
void reset()
Reset the CostHelper, returns to state at construction.
Definition CostHelper.h:87
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:266
ompl::base::Cost trueEdgeCost(const VertexConstPtrPair &edgePair) const
The true cost of an edge, including constraints.
Definition CostHelper.h:213
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",...
Definition CostHelper.h:273
ompl::base::Cost costToComeHeuristic(const VertexConstPtr &vertex) const
Calculate a heuristic estimate of the cost-to-come for a Vertex.
Definition CostHelper.h:158
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 conceptual representation of samples as an edge-implicit random geometric graph.
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition BITstar.h:137
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition BITstar.h:119
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.