Example #1
0
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);
}
Example #2
0
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;
}
Example #3
0
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);
}
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());
            }
        }
    }
}
Example #5
0
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);
    }
}
Example #7
0
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);
}