37 #include "ompl/base/objectives/StateCostIntegralObjective.h"
41 bool enableMotionCostInterpolation) :
43 interpolateMotionCost_(enableMotionCostInterpolation)
49 const State *s2)
const
51 if (interpolateMotionCost_)
53 Cost totalCost = this->identityCost();
55 int nd = si_->getStateSpace()->validSegmentCount(s1, s2);
57 State *test1 = si_->cloneState(s1);
58 Cost prevStateCost = this->stateCost(test1);
61 State *test2 = si_->allocState();
62 for (
int j = 1; j < nd; ++j)
64 si_->getStateSpace()->interpolate(s1, s2, (
double) j / (
double) nd, test2);
65 Cost nextStateCost = this->stateCost(test2);
66 totalCost.
v += this->trapezoid(prevStateCost, nextStateCost,
67 si_->distance(test1, test2)).v;
68 std::swap(test1, test2);
69 prevStateCost = nextStateCost;
71 si_->freeState(test2);
75 totalCost.
v += this->trapezoid(prevStateCost, this->stateCost(s2),
76 si_->distance(test1, s2)).v;
78 si_->freeState(test1);
83 return this->trapezoid(this->stateCost(s1), this->stateCost(s2),
84 si_->distance(s1, s2));
89 return interpolateMotionCost_;
std::string description_
The description of this optimization objective.
bool isMotionCostInterpolationEnabled(void) const
Returns whether this objective subdivides motions into smaller segments for more accurate motion cost...
double v
The value of the cost.
virtual Cost motionCost(const State *s1, const State *s2) const
Compute the cost of a path segment from s1 to s2 (including endpoints)
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition of an abstract state.
StateCostIntegralObjective(const SpaceInformationPtr &si, bool enableMotionCostInterpolation=false)
If enableMotionCostInterpolation is set to true, then calls to motionCost() will divide the motion se...
Abstract definition of optimization objectives.