ompl::base::Cost ompl::base::OptimizationObjective::costToGo(const State *state, const Goal *goal) const
{
if (hasCostToGoHeuristic())
return costToGoFn_(state, goal);
else
return identityCost(); // assumes that identity < all costs
}
ompl::base::Cost ompl::base::MultiOptimizationObjective::stateCost(const State *s) const
{
Cost c = identityCost();
for (std::vector<Component>::const_iterator comp = components_.begin();
comp != components_.end();
++comp)
{
c = Cost(c.value() + comp->weight * (comp->objective->stateCost(s).value()));
}
return c;
}
ompl::base::Cost ompl::base::OptimizationObjective::averageStateCost(unsigned int numStates) const
{
StateSamplerPtr ss = si_->allocStateSampler();
State *state = si_->allocState();
Cost totalCost(identityCost());
for (unsigned int i = 0 ; i < numStates ; ++i)
{
ss->sampleUniform(state);
totalCost = combineCosts(totalCost, stateCost(state));
}
si_->freeState(state);
return Cost(totalCost.value() / (double)numStates);
}
ompl::base::Cost ompl::base::PathLengthOptimizationObjective::stateCost(const State *) const
{
return identityCost();
}
ompl::base::Cost ompl::base::OptimizationObjective::terminalCost(const State *s) const
{
return identityCost();
}
ompl::base::Cost ompl::base::OptimizationObjective::motionCostHeuristic(const State *s1, const State *s2) const
{
return identityCost(); // assumes that identity < all costs
}
