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"
56 return this->isCostBetterThan(c, threshold_);
77 OMPL_ERROR(
"Error: Cost computation is only implemented for paths of type PathGeometric.");
78 return this->identityCost();
85 OMPL_ERROR(
"Cannot compute cost of an empty path.");
86 return this->identityCost();
91 Cost cost(this->identityCost());
92 for (std::size_t i = 1; i < numStates; ++i)
96 cost = this->combineCosts(cost, this->motionCost(s1, s2));
126 return Cost(std::numeric_limits<double>::infinity());
131 return identityCost();
136 return identityCost();
141 return si_->getStateSpace()->hasSymmetricInterpolate();
147 State *state = si_->allocState();
148 Cost totalCost(this->identityCost());
150 for (
unsigned int i = 0 ; i < numStates ; ++i)
152 ss->sampleUniform(state);
153 totalCost = this->combineCosts(totalCost, this->stateCost(state));
156 si_->freeState(state);
158 return Cost(totalCost.
v / (
double)numStates);
163 costToGoFn_ = costToGo;
167 const Goal* goal)
const
170 return costToGoFn_(state, goal);
172 return this->identityCost();
176 const State* s2)
const
178 return this->identityCost();
197 ompl::base::MultiOptimizationObjective::
198 MultiOptimizationObjective(
const SpaceInformationPtr &si) :
199 OptimizationObjective(si),
204 ompl::base::MultiOptimizationObjective::Component::
205 Component(
const OptimizationObjectivePtr& obj,
double weight) :
206 objective(obj), weight(weight)
215 throw Exception(
"This optimization objective is locked. No further objectives can be added.");
218 components_.push_back(
Component(objective, weight));
223 return components_.size();
229 if (components_.size() > idx)
230 return components_[idx].objective;
232 throw Exception(
"Objective index does not exist.");
237 if (components_.size() > idx)
238 return components_[idx].weight;
240 throw Exception(
"Objective index does not exist.");
246 if (components_.size() > idx)
247 components_[idx].weight = weight;
249 throw Exception(
"Objecitve index does not exist.");
264 Cost c = this->identityCost();
265 for (std::vector<Component>::const_iterator comp = components_.begin();
266 comp != components_.end();
269 c.
v += comp->weight*(comp->objective->stateCost(s).v);
276 const State* s2)
const
278 Cost c = this->identityCost();
279 for (std::vector<Component>::const_iterator comp = components_.begin();
280 comp != components_.end();
283 c.
v += comp->weight*(comp->objective->motionCost(s1, s2).v);
292 std::vector<MultiOptimizationObjective::Component> components;
298 for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
301 Component(mult->getObjective(i),
302 mult->getObjectiveWeight(i)));
313 for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
316 Component(mult->getObjective(i),
317 mult->getObjectiveWeight(i)));
326 for (std::vector<MultiOptimizationObjective::Component>::const_iterator comp = components.begin();
327 comp != components.end();
339 std::vector<MultiOptimizationObjective::Component> components;
345 for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i)
348 ::Component(mult->getObjective(i),
349 weight * mult->getObjectiveWeight(i)));
358 for (std::vector<MultiOptimizationObjective::Component>::const_iterator comp = components.begin();
359 comp != components.end();
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.
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...
double v
The value of the cost.
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.
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.
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.
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...
Defines a pairing of an objective and its weight.
Definition of an abstract state.
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.
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 of a geometric path.
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.
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 ...