space.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/projection.h>
#include <se2ez/plan/sampler.h>

#include <ompl/base/spaces/RealVectorStateProjections.h>

using namespace se2ez;

///
/// StateSpace::StateType
///

plan::StateSpace::StateType::StateType(const RobotPtr &robot)
  : ompl::base::RealVectorStateSpace::StateType(), state(std::make_shared<se2ez::State>(robot))
{
    values = state->data.data();
}

///
/// StateSpace
///

plan::StateSpace::StateSpace(const RobotPtr &robot)
  : ompl::base::RealVectorStateSpace(robot->getNumJoints()), robot_(robot), frames_(robot->getJoints())
{
    const auto &upper = robot_->getUpperLimits();
    const auto &lower = robot_->getLowerLimits();
    for (unsigned int i = 0; i < dimension_; ++i)
    {
        bounds_.setLow(i, lower[i]);
        bounds_.setHigh(i, upper[i]);
    }

    // Heuristic for decent VSF in serial chain manipulators
    setLongestValidSegmentFraction(0.01 / robot->getNumJoints() / 2);
}

void plan::StateSpace::enforceBounds(ompl::base::State *state) const
{
    auto *as = state->as<StateType>();
    robot_->enforceLimits(as->state);
}

bool plan::StateSpace::satisfiesBounds(const ompl::base::State *state) const
{
    const auto *as = state->as<StateType>();
    return robot_->inLimits(as->state);
}

double plan::StateSpace::distance(const ompl::base::State *state1, const ompl::base::State *state2) const
{
    const auto *as1 = state1->as<StateType>();
    const auto *as2 = state2->as<StateType>();

    return robot_->distance(as1->state, as2->state);
}

bool plan::StateSpace::equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const
{
    return distance(state1, state2) < 2 * math::eps;
}

void plan::StateSpace::interpolate(const ompl::base::State *from, const ompl::base::State *to, double t,
                                   ompl::base::State *state) const
{
    const auto *af = from->as<StateType>();
    const auto *at = to->as<StateType>();
    auto *as = state->as<StateType>();

    robot_->interpolate(af->state, at->state, t, as->state);
}

void plan::StateSpace::copyState(ompl::base::State *destination, const ompl::base::State *source) const
{
    auto *ad = destination->as<StateType>();
    const auto *as = source->as<StateType>();

    ad->state->copy(as->state);
}

ompl::base::State *plan::StateSpace::allocState() const
{
    return new StateType(robot_);
}

ompl::base::State *plan::StateSpace::allocState(const StatePtr &state) const
{
    auto *s = new StateType(robot_);
    s->state = state;

    return s;
}

void plan::StateSpace::freeState(ompl::base::State *state) const
{
    auto *as = state->as<StateType>();
    delete as;
}

void plan::StateSpace::printState(const ompl::base::State *state, std::ostream &out) const
{
    const auto *as = state->as<StateType>();
    for (const auto &pair : frames_)
    {
        const auto &frame = pair.second;
        out << frame->frame->getName() << tf::printVector(frame->getValuesConst(as->state->data))
            << std::endl;
    }

    out << std::endl;
}

double *plan::StateSpace::getValueAddressAtIndex(ompl::base::State *state, unsigned int index) const
{
    auto *as = state->as<StateType>();
    as->state->dirty = true;
    return index < dimension_ ? &as->state->data[index] : nullptr;

    // return ompl::base::RealVectorStateSpace::getValueAddressAtIndex(state, index);
}

ompl::base::StateSamplerPtr plan::StateSpace::allocDefaultStateSampler() const
{
    return std::make_shared<plan::StateSampler>(this);
}

void plan::StateSpace::registerProjections()
{
    const auto &ees = robot_->getEEs();
    for (const auto &ee : ees)
    {
        registerProjection(ee + ":R2", std::make_shared<R2Projection>(this, ee));
        registerProjection(ee + ":SE2", std::make_shared<SE2Projection>(this, ee));
    }

    if (ees.empty())
    {
        if (dimension_ > 2)
        {
            int p = std::max(2, (int)ceil(log((double)dimension_)));
            registerDefaultProjection(
                std::make_shared<ompl::base::RealVectorRandomLinearProjectionEvaluator>(this, p));
        }
        else
            registerDefaultProjection(
                std::make_shared<ompl::base::RealVectorIdentityProjectionEvaluator>(this));
    }
    else
        registerDefaultProjection(getRegisteredProjections().find(ees.front() + ":R2")->second);
}

const RobotPtr plan::StateSpace::getRobot() const
{
    return robot_;
}