OptimizationObjective.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, 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 #include "ompl/base/OptimizationObjective.h"
38 #include "ompl/tools/config/MagicConstants.h"
39 #include "ompl/base/goals/GoalRegion.h"
40 #include "ompl/base/samplers/informed/RejectionInfSampler.h"
41 #include <limits>
42 // For std::make_shared
43 #include <memory>
44 #include <utility>
45 
46 ompl::base::OptimizationObjective::OptimizationObjective(SpaceInformationPtr si) : si_(std::move(si)), threshold_(0.0)
47 {
48 }
49 
51 {
52  return description_;
53 }
54 
56 {
57  return isCostBetterThan(c, threshold_);
58 }
59 
61 {
62  return threshold_;
63 }
64 
66 {
67  threshold_ = c;
68 }
69 
71 {
72  return c1.value() < c2.value();
73 }
74 
76 {
77  // If c1 is not better than c2, and c2 is not better than c1, then they are equal
78  return !isCostBetterThan(c1, c2) && !isCostBetterThan(c2, c1);
79 }
80 
82 {
83  return isCostBetterThan(cost, infiniteCost());
84 }
85 
87 {
88  return isCostBetterThan(c1, c2) ? c1 : c2;
89 }
90 
92 {
93  return Cost(c1.value() + c2.value());
94 }
95 
97 {
98  return Cost(0.0);
99 }
100 
102 {
103  return Cost(std::numeric_limits<double>::infinity());
104 }
105 
107 {
108  return identityCost();
109 }
110 
112 {
113  return identityCost();
114 }
115 
117 {
118  return si_->getStateSpace()->hasSymmetricInterpolate();
119 }
120 
122 {
123  StateSamplerPtr ss = si_->allocStateSampler();
124  State *state = si_->allocState();
125  Cost totalCost(identityCost());
126 
127  for (unsigned int i = 0; i < numStates; ++i)
128  {
129  ss->sampleUniform(state);
130  totalCost = combineCosts(totalCost, stateCost(state));
131  }
132 
133  si_->freeState(state);
134 
135  return Cost(totalCost.value() / (double)numStates);
136 }
137 
139 {
141 }
142 
144 {
145  return static_cast<bool>(costToGoFn_);
146 }
147 
149 {
150  if (hasCostToGoHeuristic())
151  return costToGoFn_(state, goal);
152  else
153  return identityCost(); // assumes that identity < all costs
154 }
155 
157 {
158  return identityCost(); // assumes that identity < all costs
159 }
160 
162 {
163  return si_;
164 }
165 
167  const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls) const
168 {
169  OMPL_INFORM("%s: No direct informed sampling scheme is defined, defaulting to rejection sampling.",
170  description_.c_str());
171  return std::make_shared<RejectionInfSampler>(probDefn, maxNumberCalls);
172 }
173 
174 void ompl::base::OptimizationObjective::print(std::ostream &out) const
175 {
176  out << "Optimization Objective: " << description_ << " @" << this << std::endl;
177  out << "Optimization Threshold: " << threshold_ << std::endl;
178 }
179 
181 {
182  const GoalRegion *goalRegion = goal->as<GoalRegion>();
183 
184  // Ensures that all states within the goal region's threshold to
185  // have a cost-to-go of exactly zero.
186  return Cost(std::max(goalRegion->distanceGoal(state) - goalRegion->getThreshold(), 0.0));
187 }
188 
189 ompl::base::MultiOptimizationObjective::MultiOptimizationObjective(const SpaceInformationPtr &si)
190  : OptimizationObjective(si), locked_(false)
191 {
192 }
193 
194 ompl::base::MultiOptimizationObjective::Component::Component(OptimizationObjectivePtr obj, double weight)
195  : objective(std::move(obj)), weight(weight)
196 {
197 }
198 
200 {
201  if (locked_)
202  {
203  throw Exception("This optimization objective is locked. No further objectives can be added.");
204  }
205  else
206  components_.push_back(Component(objective, weight));
207 }
208 
210 {
211  return components_.size();
212 }
213 
215 {
216  if (components_.size() > idx)
217  return components_[idx].objective;
218  else
219  throw Exception("Objective index does not exist.");
220 }
221 
223 {
224  if (components_.size() > idx)
225  return components_[idx].weight;
226  else
227  throw Exception("Objective index does not exist.");
228 }
229 
231 {
232  if (components_.size() > idx)
233  components_[idx].weight = weight;
234  else
235  throw Exception("Objecitve index does not exist.");
236 }
237 
239 {
240  locked_ = true;
241 }
242 
244 {
245  return locked_;
246 }
247 
249 {
250  Cost c = identityCost();
251  for (const auto &component : components_)
252  {
253  c = Cost(c.value() + component.weight * (component.objective->stateCost(s).value()));
254  }
255 
256  return c;
257 }
258 
260 {
261  Cost c = identityCost();
262  for (const auto &component : components_)
263  {
264  c = Cost(c.value() + component.weight * (component.objective->motionCost(s1, s2).value()));
265  }
266 
267  return c;
268 }
269 
271  const OptimizationObjectivePtr &b)
272 {
273  std::vector<MultiOptimizationObjective::Component> components;
274 
275  if (a)
276  {
277  if (MultiOptimizationObjective *mult = dynamic_cast<MultiOptimizationObjective *>(a.get()))
278  {
279  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
280  {
281  components.push_back(
282  MultiOptimizationObjective::Component(mult->getObjective(i), mult->getObjectiveWeight(i)));
283  }
284  }
285  else
286  components.push_back(MultiOptimizationObjective::Component(a, 1.0));
287  }
288 
289  if (b)
290  {
291  if (MultiOptimizationObjective *mult = dynamic_cast<MultiOptimizationObjective *>(b.get()))
292  {
293  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
294  {
295  components.push_back(
296  MultiOptimizationObjective::Component(mult->getObjective(i), mult->getObjectiveWeight(i)));
297  }
298  }
299  else
300  components.push_back(MultiOptimizationObjective::Component(b, 1.0));
301  }
302 
303  auto multObj(std::make_shared<MultiOptimizationObjective>(a->getSpaceInformation()));
304  for (const auto &comp : components)
305  multObj->addObjective(comp.objective, comp.weight);
306 
307  return multObj;
308 }
309 
311 {
312  std::vector<MultiOptimizationObjective::Component> components;
313 
314  if (a)
315  {
316  if (MultiOptimizationObjective *mult = dynamic_cast<MultiOptimizationObjective *>(a.get()))
317  {
318  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
319  {
320  components.push_back(
321  MultiOptimizationObjective::Component(mult->getObjective(i), weight * mult->getObjectiveWeight(i)));
322  }
323  }
324  else
325  components.push_back(MultiOptimizationObjective::Component(a, weight));
326  }
327 
328  auto multObj(std::make_shared<MultiOptimizationObjective>(a->getSpaceInformation()));
329  for (auto const &comp : components)
330  multObj->addObjective(comp.objective, comp.weight);
331 
332  return multObj;
333 }
334 
336 {
337  return weight * a;
338 }
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...
A shared pointer wrapper for ompl::base::StateSampler.
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 double distanceGoal(const State *st) const =0
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
STL namespace.
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...
void addObjective(const OptimizationObjectivePtr &objective, double weight)
Adds a new objective for this multiobjective. A weight must also be specified for specifying importan...
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 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...
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...
bool isLocked() const
Returns whether this multiobjective has been locked from adding further objectives.
Cost stateCost(const State *s) const override
A shared pointer wrapper for ompl::base::SpaceInformation.
T * as()
Cast this instance to a desired type.
Definition: Goal.h:77
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...
void setObjectiveWeight(unsigned int idx, double weight)
Sets the weighing factor of a specific objective.
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.
double getThreshold() const
Get the distance to the goal that is allowed for a state to be considered in the goal region...
Definition: GoalRegion.h:82
The exception type for ompl.
Definition: Exception.h:46
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...
Definition of a goal region.
Definition: GoalRegion.h:47
void lock()
This method "freezes" this multiobjective so that no more objectives can be added to it...
bool hasCostToGoHeuristic() const
Check if this objective has a cost-to-go heuristic function.
double value() const
The value of the cost.
Definition: Cost.h:56
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.
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.
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.
Cost motionCost(const State *s1, const State *s2) const override
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...
std::size_t getObjectiveCount() const
Returns the number of objectives that make up this multiobjective.
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...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68