sampler.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <se2ez/core/joint.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/state.h>

#include <se2ez/plan/space.h>
#include <se2ez/plan/sampler.h>

using namespace se2ez;

plan::StateSampler::StateSampler(const plan::StateSpace *space) : ompl::base::RealVectorStateSampler(space)
{
}

void plan::StateSampler::sampleUniform(ompl::base::State *state)
{
    ompl::base::RealVectorStateSampler::sampleUniform(state);

    auto *as = state->as<plan::StateSpace::StateType>();
    as->state->dirty = true;
}

void plan::StateSampler::sampleUniformNear(ompl::base::State *state, const ompl::base::State *near,
                                           double distance)
{
    const unsigned int dim = space_->getDimension();

    auto *as = state->as<plan::StateSpace::StateType>();
    const auto *an = near->as<plan::StateSpace::StateType>();

    for (unsigned int i = 0; i < dim; ++i)
        as->values[i] = rng_.uniformReal(an->values[i] - distance, an->values[i] + distance);

    space_->enforceBounds(as);
    as->state->dirty = true;
}

void plan::StateSampler::sampleGaussian(ompl::base::State *state, const ompl::base::State *mean,
                                        double stdDev)
{
    const unsigned int dim = space_->getDimension();

    auto *as = state->as<plan::StateSpace::StateType>();
    const auto *am = mean->as<plan::StateSpace::StateType>();

    for (unsigned int i = 0; i < dim; ++i)
        as->values[i] = rng_.gaussian(am->values[i], stdDev);

    space_->enforceBounds(as);
    as->state->dirty = true;
}