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 identityCost();
94 }
95 
97 {
98  return Cost(c1.value() + c2.value());
99 }
100 
102 {
103  return Cost(0.0);
104 }
105 
107 {
108  return Cost(std::numeric_limits<double>::infinity());
109 }
110 
112 {
113  return identityCost();
114 }
115 
117 {
118  return identityCost();
119 }
120 
122 {
123  return si_->getStateSpace()->hasSymmetricInterpolate();
124 }
125 
127 {
128  StateSamplerPtr ss = si_->allocStateSampler();
129  State *state = si_->allocState();
130  Cost totalCost(identityCost());
131 
132  for (unsigned int i = 0; i < numStates; ++i)
133  {
134  ss->sampleUniform(state);
135  totalCost = combineCosts(totalCost, stateCost(state));
136  }
137 
138  si_->freeState(state);
139 
140  return Cost(totalCost.value() / (double)numStates);
141 }
142 
144 {
145  costToGoFn_ = costToGo;
146 }
147 
149 {
150  return static_cast<bool>(costToGoFn_);
151 }
152 
154 {
155  if (hasCostToGoHeuristic())
156  return costToGoFn_(state, goal);
157 
158  return identityCost(); // assumes that identity < all costs
159 }
160 
162 {
163  return identityCost(); // assumes that identity < all costs
164 }
165 
167 {
168  return si_;
169 }
170 
172  const ProblemDefinitionPtr &probDefn, unsigned int maxNumberCalls) const
173 {
174  OMPL_INFORM("%s: No direct informed sampling scheme is defined, defaulting to rejection sampling.",
175  description_.c_str());
176  return std::make_shared<RejectionInfSampler>(probDefn, maxNumberCalls);
177 }
178 
179 void ompl::base::OptimizationObjective::print(std::ostream &out) const
180 {
181  out << "Optimization Objective: " << description_ << " @" << this << std::endl;
182  out << "Optimization Threshold: " << threshold_ << std::endl;
183 }
184 
186 {
187  const auto *goalRegion = goal->as<GoalRegion>();
188 
189  // Ensures that all states within the goal region's threshold to
190  // have a cost-to-go of exactly zero.
191  return Cost(std::max(goalRegion->distanceGoal(state) - goalRegion->getThreshold(), 0.0));
192 }
193 
194 ompl::base::MultiOptimizationObjective::MultiOptimizationObjective(const SpaceInformationPtr &si)
195  : OptimizationObjective(si), locked_(false)
196 {
197 }
198 
199 ompl::base::MultiOptimizationObjective::Component::Component(OptimizationObjectivePtr obj, double weight)
200  : objective(std::move(obj)), weight(weight)
201 {
202 }
203 
205 {
206  if (locked_)
207  {
208  throw Exception("This optimization objective is locked. No further objectives can be added.");
209  }
210  else
211  components_.emplace_back(objective, weight);
212 }
213 
215 {
216  return components_.size();
217 }
218 
220 {
221  if (components_.size() > idx)
222  return components_[idx].objective;
223  throw Exception("Objective index does not exist.");
224 }
225 
227 {
228  if (components_.size() > idx)
229  return components_[idx].weight;
230  throw Exception("Objective index does not exist.");
231 }
232 
234 {
235  if (components_.size() > idx)
236  components_[idx].weight = weight;
237  else
238  throw Exception("Objecitve index does not exist.");
239 }
240 
242 {
243  locked_ = true;
244 }
245 
247 {
248  return locked_;
249 }
250 
252 {
253  Cost c = identityCost();
254  for (const auto &component : components_)
255  {
256  c = Cost(c.value() + component.weight * (component.objective->stateCost(s).value()));
257  }
258 
259  return c;
260 }
261 
263 {
264  Cost c = identityCost();
265  for (const auto &component : components_)
266  {
267  c = Cost(c.value() + component.weight * (component.objective->motionCost(s1, s2).value()));
268  }
269 
270  return c;
271 }
272 
274  const OptimizationObjectivePtr &b)
275 {
276  std::vector<MultiOptimizationObjective::Component> components;
277 
278  if (a)
279  {
280  if (auto *mult = dynamic_cast<MultiOptimizationObjective *>(a.get()))
281  {
282  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
283  {
284  components.emplace_back(mult->getObjective(i), mult->getObjectiveWeight(i));
285  }
286  }
287  else
288  components.emplace_back(a, 1.0);
289  }
290 
291  if (b)
292  {
293  if (auto *mult = dynamic_cast<MultiOptimizationObjective *>(b.get()))
294  {
295  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
296  {
297  components.emplace_back(mult->getObjective(i), mult->getObjectiveWeight(i));
298  }
299  }
300  else
301  components.emplace_back(b, 1.0);
302  }
303 
304  auto multObj(std::make_shared<MultiOptimizationObjective>(a->getSpaceInformation()));
305  for (const auto &comp : components)
306  multObj->addObjective(comp.objective, comp.weight);
307 
308  return multObj;
309 }
310 
312 {
313  std::vector<MultiOptimizationObjective::Component> components;
314 
315  if (a)
316  {
317  if (auto *mult = dynamic_cast<MultiOptimizationObjective *>(a.get()))
318  {
319  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
320  {
321  components.emplace_back(mult->getObjective(i), weight * mult->getObjectiveWeight(i));
322  }
323  }
324  else
325  components.emplace_back(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 }
Definition of a goal region.
Definition: GoalRegion.h:111
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
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.
Definition of an abstract control.
Definition: Control.h:111
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...
Cost motionCost(const State *s1, const State *s2) const override
double value() const
The value of the cost.
Definition: Cost.h:152
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...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
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...
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.
A shared pointer wrapper for ompl::base::ProblemDefinition.
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.
A shared pointer wrapper for ompl::base::StateSampler.
The exception type for ompl.
Definition: Exception.h:78
void setObjectiveWeight(unsigned int idx, double weight)
Sets the weighing factor of a specific objective.
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...