All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 */
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 <boost/noncopyable.hpp>
44 #include <boost/concept_check.hpp>
45 
46 namespace ompl
47 {
48  namespace base
49  {
50  class Goal;
51 
53  typedef boost::function<Cost (const State*, const Goal*)> CostToGoHeuristic;
54 
56 
57  OMPL_CLASS_FORWARD(OptimizationObjective);
59 
61  OMPL_CLASS_FORWARD(Path);
63 
70  class OptimizationObjective : private boost::noncopyable
71  {
72  public:
75 
76  virtual ~OptimizationObjective(void)
77  {
78  }
79 
81  const std::string& getDescription(void) const;
82 
84  virtual bool isSatisfied(Cost c) const;
85 
87  Cost getCostThreshold(void) const;
88 
90  void setCostThreshold(Cost c);
91 
93  virtual Cost getCost(const Path &path) const;
94 
96  virtual bool isCostBetterThan(Cost c1, Cost c2) const;
97 
99  virtual Cost stateCost(const State *s) const;
100 
102  virtual Cost motionCost(const State *s1, const State *s2) const = 0;
103 
105  virtual Cost combineCosts(Cost c1, Cost c2) const;
106 
108  virtual Cost identityCost() const;
109 
111  virtual Cost infiniteCost() const;
112 
114  virtual Cost initialCost(const State *s) const;
115 
117  virtual Cost terminalCost(const State *s) const;
118 
120  virtual bool isSymmetric(void) const;
121 
123  virtual Cost averageStateCost(unsigned int numStates) const;
124 
127 
129  Cost costToGo(const State* state, const Goal* goal) const;
130 
132  virtual Cost motionCostHeuristic(const State* s1, const State* s2) const;
133 
135  const SpaceInformationPtr& getSpaceInformation(void) const;
136 
137  protected:
140 
142  std::string description_;
143 
146 
149  };
150 
159  Cost goalRegionCostToGo(const State* state, const Goal* goal);
160 
163  {
164  public:
166 
168  void addObjective(const OptimizationObjectivePtr& objective,
169  double weight);
170 
172  std::size_t getObjectiveCount(void) const;
173 
175  const OptimizationObjectivePtr& getObjective(unsigned int idx) const;
176 
178  double getObjectiveWeight(unsigned int idx) const;
179 
181  void setObjectiveWeight(unsigned int idx, double weight);
182 
184  void lock(void);
185 
187  bool isLocked(void) const;
188 
193  virtual Cost stateCost(const State* s) const;
194 
199  virtual Cost motionCost(const State* s1, const State* s2) const;
200 
201  protected:
202 
204  struct Component
205  {
206  Component(const OptimizationObjectivePtr& obj, double weight);
207  OptimizationObjectivePtr objective;
208  double weight;
209  };
210 
212  std::vector<Component> components_;
213 
215  bool locked_;
216 
217  // Friend functions for operator overloads for easy multiobjective creation
219  const OptimizationObjectivePtr &b);
220 
222 
224  };
225 
228  const OptimizationObjectivePtr &b);
229 
232 
235  }
236 }
237 
238 #endif
virtual Cost initialCost(const State *s) const
Returns a cost value corresponding to starting at a state s. No optimal planners currently support th...
std::string description_
The description of this optimization objective.
const std::string & getDescription(void) const
Get the description of this optimization objective.
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
virtual bool isSymmetric(void) 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.
std::vector< Component > components_
List of objective/weight pairs.
std::size_t getObjectiveCount(void) const
Returns the number of objectives that make up this multiobjective.
boost::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 getCostThreshold(void) const
Returns the cost threshold currently being checked for objective satisfaction.
Abstract definition of goals.
Definition: Goal.h:63
Cost goalRegionCostToGo(const State *state, const Goal *goal)
For use when goal region's distanceGoal() is equivalent to the cost-to-go of a state under the optimi...
void addObjective(const OptimizationObjectivePtr &objective, double weight)
Adds a new objective for this multiobjective. A weight must also be specified for specifying importan...
virtual Cost averageStateCost(unsigned int numStates) const
Compute the average state cost of this objective by taking a sample of numStates states.
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 infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
virtual Cost motionCost(const State *s1, const State *s2) const =0
Get the cost that corresponds to the motion segment between s1 and s2.
CostToGoHeuristic costToGoFn_
The function used for returning admissible estimates on the optimal cost of the path between a given ...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
OptimizationObjective(const SpaceInformationPtr &si)
Constructor. The objective must always know the space information it is part of. The cost threshold f...
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...
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...
virtual Cost motionCost(const State *s1, const State *s2) const
virtual Cost getCost(const Path &path) const
Get the cost that corresponds to an entire path. This implementation assumes Path is of type PathGeom...
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...
Abstract definition of a path.
Definition: Path.h:67
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 on...
virtual Cost stateCost(const State *s) const
Evaluate a cost map defined on the state space at a state s. Default implementation maps all states t...
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Defines a pairing of an objective and its weight.
Definition of an abstract state.
Definition: State.h:50
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.
void lock(void)
This method "freezes" this multiobjective so that no more objectives can be added to it...
Abstract definition of optimization objectives.
friend OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
double getObjectiveWeight(unsigned int idx) const
Returns the weighing factor of a specific objective.
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
bool isLocked(void) const
Returns whether this multiobjective has been locked from adding further objectives.
bool locked_
Whether this multiobjective is locked from further additions.
virtual bool isSatisfied(Cost c) const
Verify that our objective is satisfied already and we can stop planning.
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...
const SpaceInformationPtr & getSpaceInformation(void) const
Returns this objective's SpaceInformation. Needed for operators in MultiOptimizationObjective.
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...
SpaceInformationPtr si_
The space information for this objective.
virtual Cost stateCost(const State *s) const
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 OptimizationObjectivePtr & getObjective(unsigned int idx) const
Returns a specific objective from this multiobjective, where the individual objectives are in order o...
friend OptimizationObjectivePtr operator*(double w, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
OptimizationObjectivePtr operator*(double w, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...