void ompl::base::SpaceInformation::samplesPerSecond(double &uniform, double &near, double &gaussian,
unsigned int attempts) const
{
StateSamplerPtr ss = allocStateSampler();
std::vector<State *> states(attempts + 1);
allocStates(states);
time::point start = time::now();
for (unsigned int i = 0; i < attempts; ++i)
ss->sampleUniform(states[i]);
uniform = (double)attempts / time::seconds(time::now() - start);
double d = getMaximumExtent() / 10.0;
ss->sampleUniform(states[attempts]);
start = time::now();
for (unsigned int i = 1; i <= attempts; ++i)
ss->sampleUniformNear(states[i - 1], states[i], d);
near = (double)attempts / time::seconds(time::now() - start);
start = time::now();
for (unsigned int i = 1; i <= attempts; ++i)
ss->sampleGaussian(states[i - 1], states[i], d);
gaussian = (double)attempts / time::seconds(time::now() - start);
freeStates(states);
}
ompl::base::Cost ompl::base::OptimizationObjective::averageStateCost(unsigned int numStates) const
{
StateSamplerPtr ss = si_->allocStateSampler();
State *state = si_->allocState();
Cost totalCost(this->identityCost());
for (unsigned int i = 0 ; i < numStates ; ++i)
{
ss->sampleUniform(state);
totalCost = this->combineCosts(totalCost, this->stateCost(state));
}
si_->freeState(state);
return Cost(totalCost.v / (double)numStates);
}
unsigned int ompl::base::SpaceInformation::randomBounceMotion(const StateSamplerPtr &sss, const State *start,
unsigned int steps, std::vector<State *> &states,
bool alloc) const
{
if (alloc)
{
states.resize(steps);
for (unsigned int i = 0; i < steps; ++i)
states[i] = allocState();
}
else if (states.size() < steps)
steps = states.size();
const State *prev = start;
std::pair<State *, double> lastValid;
unsigned int j = 0;
for (unsigned int i = 0; i < steps; ++i)
{
sss->sampleUniform(states[j]);
lastValid.first = states[j];
if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon())
prev = states[j++];
}
return j;
}
void ompl::base::ProjectionEvaluator::inferCellSizes(void)
{
cellSizesWereInferred_ = true;
unsigned int dim = getDimension();
if (dim > 0)
{
StateSamplerPtr sampler = space_->allocStateSampler();
State *s = space_->allocState();
EuclideanProjection proj(dim);
std::vector<double> low(dim, std::numeric_limits<double>::infinity());
std::vector<double> high(dim, -std::numeric_limits<double>::infinity());
for (unsigned int i = 0 ; i < magic::PROJECTION_EXTENTS_SAMPLES ; ++i)
{
sampler->sampleUniform(s);
project(s, proj);
for (unsigned int j = 0 ; j < dim ; ++j)
{
if (low[j] > proj[j])
low[j] = proj[j];
if (high[j] < proj[j])
high[j] = proj[j];
}
}
space_->freeState(s);
cellSizes_.resize(dim);
for (unsigned int j = 0 ; j < dim ; ++j)
{
cellSizes_[j] = (high[j] - low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
{
cellSizes_[j] = 1.0;
logWarn("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary value of 1 instead.",
j, space_->getName().c_str());
}
}
}
}
double ompl::base::SpaceInformation::averageValidMotionLength(unsigned int attempts) const
{
// take the square root here because we in fact have a nested for loop
// where each loop executes #attempts steps (the sample() function of the UniformValidStateSampler if a for loop
// too)
attempts = std::max((unsigned int)floor(sqrt((double)attempts) + 0.5), 2u);
StateSamplerPtr ss = allocStateSampler();
auto uvss(std::make_shared<UniformValidStateSampler>(this));
uvss->setNrAttempts(attempts);
State *s1 = allocState();
State *s2 = allocState();
std::pair<State *, double> lastValid;
lastValid.first = nullptr;
double d = 0.0;
unsigned int count = 0;
for (unsigned int i = 0; i < attempts; ++i)
if (uvss->sample(s1))
{
++count;
ss->sampleUniform(s2);
if (checkMotion(s1, s2, lastValid))
d += distance(s1, s2);
else
d += distance(s1, s2) * lastValid.second;
}
freeState(s2);
freeState(s1);
if (count > 0)
return d / (double)count;
else
return 0.0;
}
void ompl::base::ProjectionEvaluator::estimateBounds()
{
unsigned int dim = getDimension();
estimatedBounds_.resize(dim);
if (dim > 0)
{
StateSamplerPtr sampler = space_->allocStateSampler();
State *s = space_->allocState();
EuclideanProjection proj(dim);
estimatedBounds_.setLow(std::numeric_limits<double>::infinity());
estimatedBounds_.setHigh(-std::numeric_limits<double>::infinity());
for (unsigned int i = 0; i < magic::PROJECTION_EXTENTS_SAMPLES; ++i)
{
sampler->sampleUniform(s);
project(s, proj);
for (unsigned int j = 0; j < dim; ++j)
{
if (estimatedBounds_.low[j] > proj[j])
estimatedBounds_.low[j] = proj[j];
if (estimatedBounds_.high[j] < proj[j])
estimatedBounds_.high[j] = proj[j];
}
}
// make bounding box 10% larger (5% padding on each side)
std::vector<double> diff(estimatedBounds_.getDifference());
for (unsigned int j = 0; j < dim; ++j)
{
estimatedBounds_.low[j] -= magic::PROJECTION_EXPAND_FACTOR * diff[j];
estimatedBounds_.high[j] += magic::PROJECTION_EXPAND_FACTOR * diff[j];
}
space_->freeState(s);
}
}
double ompl::base::SpaceInformation::probabilityOfValidState(unsigned int attempts) const
{
if (attempts == 0)
return 0.0;
unsigned int valid = 0;
unsigned int invalid = 0;
StateSamplerPtr ss = allocStateSampler();
State *s = allocState();
for (unsigned int i = 0; i < attempts; ++i)
{
ss->sampleUniform(s);
if (isValid(s))
++valid;
else
++invalid;
}
freeState(s);
return (double)valid / (double)(valid + invalid);
}
