OptimizationObjective.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Willow Garage, Inc.
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 Willow Garage 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 /* Author: Luis G. Torres, Ioan Sucan, Jonathan Gammell */
36 
37 #ifndef OMPL_BASE_OPTIMIZATION_OBJECTIVE_
38 #define OMPL_BASE_OPTIMIZATION_OBJECTIVE_
39 
40 #include "ompl/base/Cost.h"
41 #include "ompl/base/SpaceInformation.h"
42 #include "ompl/util/ClassForward.h"
43 #include "ompl/base/ProblemDefinition.h"
44 #include "ompl/base/samplers/InformedStateSampler.h"
45 
46 #include <functional>
47 #include <iostream>
48 
49 namespace ompl
50 {
51  namespace base
52  {
53  class Goal;
54 
57  typedef std::function<Cost(const State *, const Goal *)> CostToGoHeuristic;
58 
60 
61  OMPL_CLASS_FORWARD(OptimizationObjective);
63 
71  {
72  public:
73  // non-copyable
75  OptimizationObjective &operator=(const OptimizationObjective &) = delete;
76 
80 
81  virtual ~OptimizationObjective() = default;
82 
84  const std::string &getDescription() const;
85 
88  virtual bool isSatisfied(Cost c) const;
89 
91  Cost getCostThreshold() const;
92 
95  void setCostThreshold(Cost c);
96 
99  virtual bool isCostBetterThan(Cost c1, Cost c2) const;
100 
104  virtual bool isCostEquivalentTo(Cost c1, Cost c2) const;
105 
107  virtual bool isFinite(Cost cost) const;
108 
110  virtual Cost betterCost(Cost c1, Cost c2) const;
111 
113  virtual Cost stateCost(const State *s) const = 0;
114 
116  virtual Cost motionCost(const State *s1, const State *s2) const = 0;
117 
120  virtual Cost combineCosts(Cost c1, Cost c2) const;
121 
127  virtual Cost identityCost() const;
128 
131  virtual Cost infiniteCost() const;
132 
135  virtual Cost initialCost(const State *s) const;
136 
139  virtual Cost terminalCost(const State *s) const;
140 
143  virtual bool isSymmetric() const;
144 
146  virtual Cost averageStateCost(unsigned int numStates) const;
147 
151  void setCostToGoHeuristic(const CostToGoHeuristic &costToGo);
152 
154  bool hasCostToGoHeuristic() const;
155 
160  Cost costToGo(const State *state, const Goal *goal) const;
161 
166  virtual Cost motionCostHeuristic(const State *s1, const State *s2) const;
167 
170 
173  virtual InformedSamplerPtr allocInformedStateSampler(const ProblemDefinitionPtr &probDefn,
174  unsigned int maxNumberCalls) const;
175 
177  virtual void print(std::ostream &out) const;
178 
179  protected:
182 
184  std::string description_;
185 
188 
191  CostToGoHeuristic costToGoFn_;
192  };
193 
202  Cost goalRegionCostToGo(const State *state, const Goal *goal);
203 
208  {
209  public:
211 
214  void addObjective(const OptimizationObjectivePtr &objective, double weight);
215 
217  std::size_t getObjectiveCount() const;
218 
221  const OptimizationObjectivePtr &getObjective(unsigned int idx) const;
222 
224  double getObjectiveWeight(unsigned int idx) const;
225 
227  void setObjectiveWeight(unsigned int idx, double weight);
228 
230  void lock();
231 
233  bool isLocked() const;
234 
239  Cost stateCost(const State *s) const override;
240 
245  Cost motionCost(const State *s1, const State *s2) const override;
246 
247  protected:
249  struct Component
250  {
251  Component(OptimizationObjectivePtr obj, double weight);
252  OptimizationObjectivePtr objective;
253  double weight;
254  };
255 
257  std::vector<Component> components_;
258 
260  bool locked_;
261 
262  // Friend functions for operator overloads for easy multiobjective creation
264  const OptimizationObjectivePtr &b);
265 
267 
269  };
270 
274 
278 
282  }
283 }
284 
285 #endif
std::string description_
The description of this optimization objective.
virtual Cost initialCost(const State *s) const
Returns a cost value corresponding to starting at a state s. No optimal planners currently support th...
virtual bool isFinite(Cost cost) const
Returns whether the cost is finite or not.
A shared pointer wrapper for ompl::base::ProblemDefinition.
virtual Cost stateCost(const State *s) const =0
Evaluate a cost map defined on the state space at a state s.
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
std::vector< Component > components_
List of objective/weight pairs.
virtual bool isSatisfied(Cost c) const
Check if the the given cost c satisfies the specified cost objective, defined as better than the spec...
Abstract definition of goals.
Definition: Goal.h:62
virtual Cost identityCost() const
Get the identity cost value. The identity cost value is the cost c_i such that, for all costs c...
Cost goalRegionCostToGo(const State *state, const Goal *goal)
For use when the cost-to-go of a state under the optimization objective is equivalent to the goal reg...
Cost getCostThreshold() const
Returns the cost threshold currently being checked for objective satisfaction.
virtual Cost infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
void setCostThreshold(Cost c)
Set the cost threshold for objective satisfaction. When a path is found with a cost better than the c...
virtual Cost betterCost(Cost c1, Cost c2) const
Return the minimum cost given c1 and c2. Uses isCostBetterThan.
virtual Cost motionCost(const State *s1, const State *s2) const =0
Get the cost that corresponds to the motion segment between s1 and s2.
virtual bool isCostEquivalentTo(Cost c1, Cost c2) const
Compare whether cost c1 and cost c2 are equivalent. By default defined as !isCostBetterThan(c1, c2) && !isCostBetterThan(c2, c1), as if c1 is not better than c2, and c2 is not better than c1, then they are equal.
CostToGoHeuristic costToGoFn_
The function used for returning admissible estimates on the optimal cost of the path between a given ...
virtual Cost motionCostHeuristic(const State *s1, const State *s2) const
Defines an admissible estimate on the optimal cost on the motion between states s1 and s2...
std::function< Cost(const State *, const Goal *)> CostToGoHeuristic
The definition of a function which returns an admissible estimate of the optimal path cost from a giv...
Cost costToGo(const State *state, const Goal *goal) const
Uses a cost-to-go heuristic to calculate an admissible estimate of the optimal cost from a given stat...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void setCostToGoHeuristic(const CostToGoHeuristic &costToGo)
Set the cost-to-go heuristic function for this objective. The cost-to-go heuristic is a function whic...
Cost threshold_
The cost threshold used for checking whether this objective has been satisfied during planning...
A shared pointer wrapper for ompl::base::SpaceInformation.
Defines a pairing of an objective and its weight.
Definition of an abstract state.
Definition: State.h:49
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
virtual Cost averageStateCost(unsigned int numStates) const
Compute the average state cost of this objective by taking a sample of numStates states.
Abstract definition of optimization objectives.
A shared pointer wrapper for ompl::base::OptimizationObjective.
virtual Cost terminalCost(const State *s) const
Returns a cost value corresponding to a path ending at a state s. No optimal planners currently suppo...
virtual bool isCostBetterThan(Cost c1, Cost c2) const
Check whether the the cost c1 is considered better than the cost c2. By default, this returns true if...
bool hasCostToGoHeuristic() const
Check if this objective has a cost-to-go heuristic function.
bool locked_
Whether this multiobjective is locked from further additions.
const SpaceInformationPtr & getSpaceInformation() const
Returns this objective&#39;s SpaceInformation. Needed for operators in MultiOptimizationObjective.
virtual bool isSymmetric() const
Check if this objective has a symmetric cost metric, i.e. motionCost(s1, s2) = motionCost(s2, s1). Default implementation returns whether the underlying state space has symmetric interpolation.
virtual Cost combineCosts(Cost c1, Cost c2) const
Get the cost that corresponds to combining the costs c1 and c2. Default implementation defines this c...
SpaceInformationPtr si_
The space information for this objective.
const std::string & getDescription() const
Get the description of this optimization objective.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
virtual void print(std::ostream &out) const
Print information about this optimization objective.
OptimizationObjectivePtr operator*(double w, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
virtual InformedSamplerPtr allocInformedStateSampler(const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls) const
Allocate a heuristic-sampling state generator for this cost function, defaults to a basic rejection s...