All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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/geometric/PathGeometric.h"
39 #include "ompl/tools/config/MagicConstants.h"
40 #include "ompl/base/goals/GoalRegion.h"
41 #include <limits>
42 
44  si_(si),
45  threshold_(0.0)
46 {
47 }
48 
50 {
51  return description_;
52 }
53 
55 {
56  return this->isCostBetterThan(c, threshold_);
57 }
58 
60 {
61  return threshold_;
62 }
63 
65 {
66  threshold_ = c;
67 }
68 
70 {
71  // Cast path down to a PathGeometric
72  const geometric::PathGeometric *pathGeom = dynamic_cast<const geometric::PathGeometric*>(&path);
73 
74  // Give up if this isn't a PathGeometric or if the path is empty.
75  if (!pathGeom)
76  {
77  OMPL_ERROR("Error: Cost computation is only implemented for paths of type PathGeometric.");
78  return this->identityCost();
79  }
80  else
81  {
82  std::size_t numStates = pathGeom->getStateCount();
83  if (numStates == 0)
84  {
85  OMPL_ERROR("Cannot compute cost of an empty path.");
86  return this->identityCost();
87  }
88  else
89  {
90  // Compute path cost by accumulating the cost along the path
91  Cost cost(this->identityCost());
92  for (std::size_t i = 1; i < numStates; ++i)
93  {
94  const State *s1 = pathGeom->getState(i-1);
95  const State *s2 = pathGeom->getState(i);
96  cost = this->combineCosts(cost, this->motionCost(s1, s2));
97  }
98 
99  return cost;
100  }
101  }
102 }
103 
105 {
106  return c1.v + magic::BETTER_PATH_COST_MARGIN < c2.v;
107 }
108 
110 {
111  return Cost(1.0);
112 }
113 
115 {
116  return Cost(c1.v + c2.v);
117 }
118 
120 {
121  return Cost(0.0);
122 }
123 
125 {
126  return Cost(std::numeric_limits<double>::infinity());
127 }
128 
130 {
131  return identityCost();
132 }
133 
135 {
136  return identityCost();
137 }
138 
140 {
141  return si_->getStateSpace()->hasSymmetricInterpolate();
142 }
143 
145 {
146  StateSamplerPtr ss = si_->allocStateSampler();
147  State *state = si_->allocState();
148  Cost totalCost(this->identityCost());
149 
150  for (unsigned int i = 0 ; i < numStates ; ++i)
151  {
152  ss->sampleUniform(state);
153  totalCost = this->combineCosts(totalCost, this->stateCost(state));
154  }
155 
156  si_->freeState(state);
157 
158  return Cost(totalCost.v / (double)numStates);
159 }
160 
162 {
163  costToGoFn_ = costToGo;
164 }
165 
167  const Goal* goal) const
168 {
169  if (costToGoFn_)
170  return costToGoFn_(state, goal);
171  else
172  return this->identityCost(); // assumes that identity < all costs
173 }
174 
176  const State* s2) const
177 {
178  return this->identityCost(); // assumes that identity < all costs
179 }
180 
183 {
184  return si_;
185 }
186 
188 {
189  const GoalRegion* goalRegion = goal->as<GoalRegion>();
190 
191  // Ensures that all states within the goal region's threshold to
192  // have a cost-to-go of exactly zero.
193  return Cost(std::max(goalRegion->distanceGoal(state) - goalRegion->getThreshold(),
194  0.0));
195 }
196 
197 ompl::base::MultiOptimizationObjective::
198 MultiOptimizationObjective(const SpaceInformationPtr &si) :
199  OptimizationObjective(si),
200  locked_(false)
201 {
202 }
203 
204 ompl::base::MultiOptimizationObjective::Component::
205 Component(const OptimizationObjectivePtr& obj, double weight) :
206  objective(obj), weight(weight)
207 {
208 }
209 
211  double weight)
212 {
213  if (locked_)
214  {
215  throw Exception("This optimization objective is locked. No further objectives can be added.");
216  }
217  else
218  components_.push_back(Component(objective, weight));
219 }
220 
222 {
223  return components_.size();
224 }
225 
228 {
229  if (components_.size() > idx)
230  return components_[idx].objective;
231  else
232  throw Exception("Objective index does not exist.");
233 }
234 
236 {
237  if (components_.size() > idx)
238  return components_[idx].weight;
239  else
240  throw Exception("Objective index does not exist.");
241 }
242 
244  double weight)
245 {
246  if (components_.size() > idx)
247  components_[idx].weight = weight;
248  else
249  throw Exception("Objecitve index does not exist.");
250 }
251 
253 {
254  locked_ = true;
255 }
256 
258 {
259  return locked_;
260 }
261 
263 {
264  Cost c = this->identityCost();
265  for (std::vector<Component>::const_iterator comp = components_.begin();
266  comp != components_.end();
267  ++comp)
268  {
269  c.v += comp->weight*(comp->objective->stateCost(s).v);
270  }
271 
272  return c;
273 }
274 
276  const State* s2) const
277 {
278  Cost c = this->identityCost();
279  for (std::vector<Component>::const_iterator comp = components_.begin();
280  comp != components_.end();
281  ++comp)
282  {
283  c.v += comp->weight*(comp->objective->motionCost(s1, s2).v);
284  }
285 
286  return c;
287 }
288 
290  const OptimizationObjectivePtr &b)
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  mult->getObjectiveWeight(i)));
303  }
304  }
305  else
306  components.push_back(MultiOptimizationObjective::Component(a, 1.0));
307  }
308 
309  if (b)
310  {
311  if (MultiOptimizationObjective* mult = dynamic_cast<MultiOptimizationObjective*>(b.get()))
312  {
313  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
314  {
315  components.push_back(MultiOptimizationObjective::
316  Component(mult->getObjective(i),
317  mult->getObjectiveWeight(i)));
318  }
319  }
320  else
321  components.push_back(MultiOptimizationObjective::Component(b, 1.0));
322  }
323 
324  MultiOptimizationObjective* multObj = new MultiOptimizationObjective(a->getSpaceInformation());
325 
326  for (std::vector<MultiOptimizationObjective::Component>::const_iterator comp = components.begin();
327  comp != components.end();
328  ++comp)
329  {
330  multObj->addObjective(comp->objective, comp->weight);
331  }
332 
333  return OptimizationObjectivePtr(multObj);
334 }
335 
337  const OptimizationObjectivePtr &a)
338 {
339  std::vector<MultiOptimizationObjective::Component> components;
340 
341  if (a)
342  {
343  if (MultiOptimizationObjective* mult = dynamic_cast<MultiOptimizationObjective*>(a.get()))
344  {
345  for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
346  {
347  components.push_back(MultiOptimizationObjective
348  ::Component(mult->getObjective(i),
349  weight * mult->getObjectiveWeight(i)));
350  }
351  }
352  else
353  components.push_back(MultiOptimizationObjective::Component(a, weight));
354  }
355 
356  MultiOptimizationObjective* multObj = new MultiOptimizationObjective(a->getSpaceInformation());
357 
358  for (std::vector<MultiOptimizationObjective::Component>::const_iterator comp = components.begin();
359  comp != components.end();
360  ++comp)
361  {
362  multObj->addObjective(comp->objective, comp->weight);
363  }
364 
365  return OptimizationObjectivePtr(multObj);
366 }
367 
369  double weight)
370 {
371  return weight * a;
372 }
virtual Cost initialCost(const State *s) const
Returns a cost value corresponding to starting at a state s. No optimal planners currently support th...
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.
A boost shared pointer wrapper for ompl::base::StateSampler.
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
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...
double getThreshold(void) const
Get the distance to the goal that is allowed for a state to be considered in the goal region...
Definition: GoalRegion.h:88
double v
The value of the cost.
Definition: Cost.h:52
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...
base::State * getState(unsigned int index)
Get the state located at index along the path.
virtual Cost infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
std::size_t getStateCount(void) const
Get the number of states (way-points) that make up this path.
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...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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...
double getObjectiveWeight(unsigned int idx) const
Returns the weighing factor of a specific objective.
The exception type for ompl.
Definition: Exception.h:47
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
bool isLocked(void) const
Returns whether this multiobjective has been locked from adding further objectives.
Definition of a goal region.
Definition: GoalRegion.h:50
Definition of a geometric path.
Definition: PathGeometric.h:55
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...
T * as(void)
Cast this instance to a desired type.
Definition: Goal.h:77
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...
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 ...