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 */
36 
37 #include "ompl/base/OptimizationObjective.h"
38 #include "ompl/tools/config/MagicConstants.h"
39 #include "ompl/base/goals/GoalRegion.h"
40 #include <limits>
41 
43  si_(si),
44  threshold_(0.0)
45 {
46 }
47 
49 {
50  return description_;
51 }
52 
54 {
55  return this->isCostBetterThan(c, threshold_);
56 }
57 
59 {
60  return threshold_;
61 }
62 
64 {
65  threshold_ = c;
66 }
67 
69 {
70  return c1.value() + magic::BETTER_PATH_COST_MARGIN < c2.value();
71 }
72 
74 {
75  return Cost(c1.value() + c2.value());
76 }
77 
79 {
80  return Cost(0.0);
81 }
82 
84 {
85  return Cost(std::numeric_limits<double>::infinity());
86 }
87 
89 {
90  return identityCost();
91 }
92 
94 {
95  return identityCost();
96 }
97 
99 {
100  return si_->getStateSpace()->hasSymmetricInterpolate();
101 }
102 
104 {
105  StateSamplerPtr ss = si_->allocStateSampler();
106  State *state = si_->allocState();
107  Cost totalCost(this->identityCost());
108 
109  for (unsigned int i = 0 ; i < numStates ; ++i)
110  {
111  ss->sampleUniform(state);
112  totalCost = this->combineCosts(totalCost, this->stateCost(state));
113  }
114 
115  si_->freeState(state);
116 
117  return Cost(totalCost.value() / (double)numStates);
118 }
119 
121 {
122  costToGoFn_ = costToGo;
123 }
124 
126 {
127  if (costToGoFn_)
128  return costToGoFn_(state, goal);
129  else
130  return this->identityCost(); // assumes that identity < all costs
131 }
132 
134 {
135  return this->identityCost(); // assumes that identity < all costs
136 }
137 
139 {
140  return si_;
141 }
142 
144 {
145  const GoalRegion *goalRegion = goal->as<GoalRegion>();
146 
147  // Ensures that all states within the goal region's threshold to
148  // have a cost-to-go of exactly zero.
149  return Cost(std::max(goalRegion->distanceGoal(state) - goalRegion->getThreshold(),
150  0.0));
151 }
152 
153 ompl::base::MultiOptimizationObjective::MultiOptimizationObjective(const SpaceInformationPtr &si) :
154  OptimizationObjective(si),
155  locked_(false)
156 {
157 }
158 
159 ompl::base::MultiOptimizationObjective::Component::
160 Component(const OptimizationObjectivePtr& obj, double weight) :
161  objective(obj), weight(weight)
162 {
163 }
164 
166  double weight)
167 {
168  if (locked_)
169  {
170  throw Exception("This optimization objective is locked. No further objectives can be added.");
171  }
172  else
173  components_.push_back(Component(objective, weight));
174 }
175 
177 {
178  return components_.size();
179 }
180 
182 {
183  if (components_.size() > idx)
184  return components_[idx].objective;
185  else
186  throw Exception("Objective index does not exist.");
187 }
188 
190 {
191  if (components_.size() > idx)
192  return components_[idx].weight;
193  else
194  throw Exception("Objective index does not exist.");
195 }
196 
198  double weight)
199 {
200  if (components_.size() > idx)
201  components_[idx].weight = weight;
202  else
203  throw Exception("Objecitve index does not exist.");
204 }
205 
207 {
208  locked_ = true;
209 }
210 
212 {
213  return locked_;
214 }
215 
217 {
218  Cost c = this->identityCost();
219  for (std::vector<Component>::const_iterator comp = components_.begin();
220  comp != components_.end();
221  ++comp)
222  {
223  c = Cost(c.value() + comp->weight * (comp->objective->stateCost(s).value()));
224  }
225 
226  return c;
227 }
228 
230  const State *s2) const
231 {
232  Cost c = this->identityCost();
233  for (std::vector<Component>::const_iterator comp = components_.begin();
234  comp != components_.end();
235  ++comp)
236  {
237  c = Cost(c.value() + comp->weight * (comp->objective->motionCost(s1, s2).value()));
238  }
239 
240  return c;
241 }
242 
244  const OptimizationObjectivePtr &b)
245 {
246  std::vector<MultiOptimizationObjective::Component> components;
247 
248  if (a)
249  {
250  if (MultiOptimizationObjective *mult = dynamic_cast<MultiOptimizationObjective*>(a.get()))
251  {
252  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
253  {
254  components.push_back(MultiOptimizationObjective::
255  Component(mult->getObjective(i),
256  mult->getObjectiveWeight(i)));
257  }
258  }
259  else
260  components.push_back(MultiOptimizationObjective::Component(a, 1.0));
261  }
262 
263  if (b)
264  {
265  if (MultiOptimizationObjective *mult = dynamic_cast<MultiOptimizationObjective*>(b.get()))
266  {
267  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
268  {
269  components.push_back(MultiOptimizationObjective::Component(mult->getObjective(i),
270  mult->getObjectiveWeight(i)));
271  }
272  }
273  else
274  components.push_back(MultiOptimizationObjective::Component(b, 1.0));
275  }
276 
277  MultiOptimizationObjective *multObj = new MultiOptimizationObjective(a->getSpaceInformation());
278 
279  for (std::vector<MultiOptimizationObjective::Component>::const_iterator comp = components.begin();
280  comp != components.end();
281  ++comp)
282  {
283  multObj->addObjective(comp->objective, comp->weight);
284  }
285 
286  return OptimizationObjectivePtr(multObj);
287 }
288 
290  const OptimizationObjectivePtr &a)
291 {
292  std::vector<MultiOptimizationObjective::Component> components;
293 
294  if (a)
295  {
296  if (MultiOptimizationObjective *mult = dynamic_cast<MultiOptimizationObjective*>(a.get()))
297  {
298  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
299  {
300  components.push_back(MultiOptimizationObjective
301  ::Component(mult->getObjective(i),
302  weight * mult->getObjectiveWeight(i)));
303  }
304  }
305  else
306  components.push_back(MultiOptimizationObjective::Component(a, weight));
307  }
308 
309  MultiOptimizationObjective *multObj = new MultiOptimizationObjective(a->getSpaceInformation());
310 
311  for (std::vector<MultiOptimizationObjective::Component>::const_iterator comp = components.begin();
312  comp != components.end();
313  ++comp)
314  {
315  multObj->addObjective(comp->objective, comp->weight);
316  }
317 
318  return OptimizationObjectivePtr(multObj);
319 }
320 
322  double weight)
323 {
324  return weight * a;
325 }
virtual Cost initialCost(const State *s) const
Returns a cost value corresponding to starting at a state s. No optimal planners currently support th...
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
A boost shared pointer wrapper for ompl::base::StateSampler.
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.
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...
Abstract definition of goals.
Definition: Goal.h:63
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...
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...
const std::string & getDescription() const
Get the description of this optimization objective.
virtual Cost infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
const SpaceInformationPtr & getSpaceInformation() const
Returns this objective's SpaceInformation. Needed for operators in MultiOptimizationObjective.
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...
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
Cost getCostThreshold() const
Returns the cost threshold currently being checked for objective satisfaction.
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...
double value() const
The value of the cost.
Definition: Cost.h:54
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...
A boost 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: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.
double getObjectiveWeight(unsigned int idx) const
Returns the weighing factor of a specific objective.
The exception type for ompl.
Definition: Exception.h:47
bool isLocked() const
Returns whether this multiobjective has been locked from adding further objectives.
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
std::size_t getObjectiveCount() const
Returns the number of objectives that make up this multiobjective.
Definition of a goal region.
Definition: GoalRegion.h:50
void lock()
This method "freezes" this multiobjective so that no more objectives can be added to it...
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:88
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...
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 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...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
OptimizationObjectivePtr operator*(double w, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
static const double BETTER_PATH_COST_MARGIN
When running algorithms such as RRT*, rewire updates are made when the cost of a path appears better ...