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 // This is needed to correctly generate the python bindings
46 #ifndef __castxml__
47 #include "ompl/control/Control.h"
48 #endif
49 
50 #include <functional>
51 #include <iostream>
52 
53 namespace ompl
54 {
55  namespace base
56  {
57  class Goal;
58 
61  using CostToGoHeuristic = std::function<Cost(const State *, const Goal *)>;
62 
64 
65  OMPL_CLASS_FORWARD(OptimizationObjective);
67 
74  class OptimizationObjective
75  {
76  public:
77  // non-copyable
78  OptimizationObjective(const OptimizationObjective &) = delete;
79  OptimizationObjective &operator=(const OptimizationObjective &) = delete;
80 
83  OptimizationObjective(SpaceInformationPtr si);
84 
85  virtual ~OptimizationObjective() = default;
86 
88  const std::string &getDescription() const;
89 
92  virtual bool isSatisfied(Cost c) const;
93 
95  Cost getCostThreshold() const;
96 
99  void setCostThreshold(Cost c);
100 
103  virtual bool isCostBetterThan(Cost c1, Cost c2) const;
104 
108  virtual bool isCostEquivalentTo(Cost c1, Cost c2) const;
109 
111  virtual bool isFinite(Cost cost) const;
112 
114  virtual Cost betterCost(Cost c1, Cost c2) const;
115 
117  virtual Cost stateCost(const State *s) const = 0;
118 
120  virtual Cost motionCost(const State *s1, const State *s2) const = 0;
121 
122 // This is needed to correctly generate the python bindings
123 #ifndef __castxml__
124 
126  virtual Cost controlCost(const control::Control *c, unsigned int steps) const;
127 #endif
128 
131  virtual Cost combineCosts(Cost c1, Cost c2) const;
132 
138  virtual Cost identityCost() const;
139 
142  virtual Cost infiniteCost() const;
143 
146  virtual Cost initialCost(const State *s) const;
147 
150  virtual Cost terminalCost(const State *s) const;
151 
154  virtual bool isSymmetric() const;
155 
157  virtual Cost averageStateCost(unsigned int numStates) const;
158 
163 
165  bool hasCostToGoHeuristic() const;
166 
171  Cost costToGo(const State *state, const Goal *goal) const;
172 
177  virtual Cost motionCostHeuristic(const State *s1, const State *s2) const;
178 
181 
184  virtual InformedSamplerPtr allocInformedStateSampler(const ProblemDefinitionPtr &probDefn,
185  unsigned int maxNumberCalls) const;
186 
188  virtual void print(std::ostream &out) const;
189 
190  protected:
193 
195  std::string description_;
196 
199 
203  };
204 
213  Cost goalRegionCostToGo(const State *state, const Goal *goal);
214 
219  {
220  public:
222 
225  void addObjective(const OptimizationObjectivePtr &objective, double weight);
226 
228  std::size_t getObjectiveCount() const;
229 
232  const OptimizationObjectivePtr &getObjective(unsigned int idx) const;
233 
235  double getObjectiveWeight(unsigned int idx) const;
236 
238  void setObjectiveWeight(unsigned int idx, double weight);
239 
241  void lock();
242 
244  bool isLocked() const;
245 
250  Cost stateCost(const State *s) const override;
251 
256  Cost motionCost(const State *s1, const State *s2) const override;
257 
258  protected:
260  struct Component
261  {
262  Component(OptimizationObjectivePtr obj, double weight);
263  OptimizationObjectivePtr objective;
264  double weight;
265  };
266 
268  std::vector<Component> components_;
269 
271  bool locked_;
272 
273 
274  // Friend functions for operator overloads for easy multiobjective creation
276  const OptimizationObjectivePtr &b);
277 
278  friend OptimizationObjectivePtr operator*(double weight, const OptimizationObjectivePtr &a);
279 
280  friend OptimizationObjectivePtr operator*(const OptimizationObjectivePtr &a, double weight);
281  };
282 
286  const OptimizationObjectivePtr &b);
287 
291 
295  }
296 }
297 
298 #endif
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
virtual Cost stateCost(const State *s) const =0
Evaluate a cost map defined on the state space at a state s.
SpaceInformationPtr si_
The space information for this objective.
virtual Cost identityCost() const
Get the identity cost value. The identity cost value is the cost c_i such that, for all costs c,...
const SpaceInformationPtr & getSpaceInformation() const
Returns this objective's SpaceInformation. Needed for operators in MultiOptimizationObjective.
A shared pointer wrapper for ompl::base::SpaceInformation.
virtual bool isSatisfied(Cost c) const
Check if the the given cost c satisfies the specified cost objective, defined as better than the spec...
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...
Cost stateCost(const State *s) const override
Definition of an abstract state.
Definition: State.h:113
const std::string & getDescription() const
Get the description of this optimization objective.
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...
void setCostToGoHeuristic(const CostToGoHeuristic &costToGo)
Set the cost-to-go heuristic function for this objective. The cost-to-go heuristic is a function whic...
CostToGoHeuristic costToGoFn_
The function used for returning admissible estimates on the optimal cost of the path between a given ...
Cost motionCost(const State *s1, const State *s2) const override
bool hasCostToGoHeuristic() const
Check if this objective has a cost-to-go heuristic function.
virtual Cost infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
friend OptimizationObjectivePtr operator*(double weight, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
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.
bool isLocked() const
Returns whether this multiobjective has been locked from adding further objectives.
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...
Cost threshold_
The cost threshold used for checking whether this objective has been satisfied during planning.
Abstract definition of optimization objectives.
bool locked_
Whether this multiobjective is locked from further additions.
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...
const OptimizationObjectivePtr & getObjective(unsigned int idx) const
Returns a specific objective from this multiobjective, where the individual objectives are in order o...
double getObjectiveWeight(unsigned int idx) const
Returns the weighing factor of a specific objective.
void lock()
This method "freezes" this multiobjective so that no more objectives can be added to it.
virtual Cost motionCost(const State *s1, const State *s2) const =0
Get the cost that corresponds to the motion segment between s1 and s2.
A shared pointer wrapper for ompl::base::ProblemDefinition.
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 isSymmetric() const
Check if this objective has a symmetric cost metric, i.e. motionCost(s1, s2) = motionCost(s2,...
virtual Cost controlCost(const control::Control *c, unsigned int steps) const
Get the cost that corresponds to the motion created by a control c applied for duration steps....
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...
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....
Abstract definition of goals.
Definition: Goal.h:126
void setCostThreshold(Cost c)
Set the cost threshold for objective satisfaction. When a path is found with a cost better than the c...
virtual bool isCostEquivalentTo(Cost c1, Cost c2) const
Compare whether cost c1 and cost c2 are equivalent. By default defined as !isCostBetterThan(c1,...
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...
Cost getCostThreshold() const
Returns the cost threshold currently being checked for objective satisfaction.
void addObjective(const OptimizationObjectivePtr &objective, double weight)
Adds a new objective for this multiobjective. A weight must also be specified for specifying importan...
virtual void print(std::ostream &out) const
Print information about this optimization objective.
virtual bool isFinite(Cost cost) const
Returns whether the cost is finite or not.
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
virtual Cost betterCost(Cost c1, Cost c2) const
Return the minimum cost given c1 and c2. Uses isCostBetterThan.
virtual Cost averageStateCost(unsigned int numStates) const
Compute the average state cost of this objective by taking a sample of numStates states.
friend OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
void setObjectiveWeight(unsigned int idx, double weight)
Sets the weighing factor of a specific objective.
std::vector< Component > components_
List of objective/weight pairs.
Main namespace. Contains everything in this library.
std::size_t getObjectiveCount() const
Returns the number of objectives that make up this multiobjective.
OptimizationObjectivePtr operator*(double weight, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
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...