Loading...
Searching...
No Matches
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
46ompl::base::OptimizationObjective::OptimizationObjective(SpaceInformationPtr si) : si_(std::move(si)), threshold_(0.0)
47{
48}
49
51{
52 return description_;
53}
54
59
64
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
85
90
95
100
105
107{
108 return Cost(std::numeric_limits<double>::infinity());
109}
110
115
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
147
149{
150 return static_cast<bool>(costToGoFn_);
151}
152
154{
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
166const ompl::base::SpaceInformationPtr &ompl::base::OptimizationObjective::getSpaceInformation() const
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
179void 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
194ompl::base::MultiOptimizationObjective::MultiOptimizationObjective(const SpaceInformationPtr &si)
195 : OptimizationObjective(si), locked_(false)
196{
197}
198
199ompl::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
219const ompl::base::OptimizationObjectivePtr &ompl::base::MultiOptimizationObjective::getObjective(unsigned int idx) const
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
245
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
273ompl::base::OptimizationObjectivePtr ompl::base::operator+(const OptimizationObjectivePtr &a,
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
311ompl::base::OptimizationObjectivePtr ompl::base::operator*(double weight, const OptimizationObjectivePtr &a)
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
335ompl::base::OptimizationObjectivePtr ompl::base::operator*(const OptimizationObjectivePtr &a, double weight)
336{
337 return weight * a;
338}
The exception type for ompl.
Definition Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
double value() const
The value of the cost.
Definition Cost.h:56
Definition of a goal region.
Definition GoalRegion.h:48
Abstract definition of goals.
Definition Goal.h:63
T * as()
Cast this instance to a desired type.
Definition Goal.h:77
This class allows for the definition of multiobjective optimal planning problems. Objectives are adde...
const OptimizationObjectivePtr & getObjective(unsigned int idx) const
Returns a specific objective from this multiobjective, where the individual objectives are in order o...
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.
Cost stateCost(const State *s) const override
double getObjectiveWeight(unsigned int idx) const
Returns the weighing factor of a specific objective.
bool locked_
Whether this multiobjective is locked from further additions.
bool isLocked() const
Returns whether this multiobjective has been locked from adding further objectives.
std::vector< Component > components_
List of objective/weight pairs.
void addObjective(const OptimizationObjectivePtr &objective, double weight)
Adds a new objective for this multiobjective. A weight must also be specified for specifying importan...
Cost motionCost(const State *s1, const State *s2) const override
void lock()
This method "freezes" this multiobjective so that no more objectives can be added to it.
A shared pointer wrapper for ompl::base::OptimizationObjective.
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...
std::string description_
The description of this optimization objective.
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 averageStateCost(unsigned int numStates) const
Compute the average state cost of this objective by taking a sample of numStates states.
bool hasCostToGoHeuristic() const
Check if this objective has a cost-to-go heuristic function.
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 isFinite(Cost cost) const
Returns whether the cost is finite or not.
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 stateCost(const State *s) const =0
Evaluate a cost map defined on the state space at a state s.
const SpaceInformationPtr & getSpaceInformation() const
Returns this objective's SpaceInformation. Needed for operators in MultiOptimizationObjective.
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 identityCost() const
Get the identity cost value. The identity cost value is the cost c_i such that, for all costs c,...
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 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....
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...
CostToGoHeuristic costToGoFn_
The function used for returning admissible estimates on the optimal cost of the path between a given ...
Cost getCostThreshold() const
Returns the cost threshold currently being checked for objective satisfaction.
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 betterCost(Cost c1, Cost c2) const
Return the minimum cost given c1 and c2. Uses isCostBetterThan.
virtual bool isSymmetric() const
Check if this objective has a symmetric cost metric, i.e. motionCost(s1, s2) = motionCost(s2,...
virtual Cost infiniteCost() const
Get a cost which is greater than all other costs in this OptimizationObjective; required for use in D...
const std::string & getDescription() const
Get 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 void print(std::ostream &out) const
Print information about this optimization objective.
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...
SpaceInformationPtr si_
The space information for this objective.
A shared pointer wrapper for ompl::base::ProblemDefinition.
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::base::StateSampler.
Definition of an abstract state.
Definition State.h:50
Definition of an abstract control.
Definition Control.h:48
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
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...
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...
OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a, const OptimizationObjectivePtr &b)
Given two optimization objectives, returns a MultiOptimizationObjective that combines the two objecti...
OptimizationObjectivePtr operator*(double weight, const OptimizationObjectivePtr &a)
Given a weighing factor and an optimization objective, returns a MultiOptimizationObjective containin...
STL namespace.