goals.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <se2ez/core/log.h>
#include <se2ez/core/math.h>

#include <se2ez/plan/goals.h>

#include <ompl/base/ConstrainedSpaceInformation.h>

using namespace ompl::base;

WrapperGoalRegion::WrapperGoalRegion(const SpaceInformationPtr &si, const std::shared_ptr<GoalRegion> &goal)
  : GoalRegion(si), goal_(goal)
{
}

bool WrapperGoalRegion::isSatisfied(const State *state) const
{
    return goal_->isSatisfied(state->as<WrapperStateSpace::StateType>()->getState());
}

bool WrapperGoalRegion::isSatisfied(const State *state, double *distance) const
{
    return goal_->isSatisfied(state->as<WrapperStateSpace::StateType>()->getState(), distance);
}

double WrapperGoalRegion::distanceGoal(const State *state) const
{
    return goal_->distanceGoal(state->as<WrapperStateSpace::StateType>()->getState());
}

void WrapperGoalRegion::print(std::ostream &out) const
{
    goal_->print(out);
}

void WrapperGoalRegion::setThreshold(double threshold)
{
    goal_->setThreshold(threshold);
}

double WrapperGoalRegion::getThreshold() const
{
    return goal_->getThreshold();
}

///
/// GoalConstraint
///

GoalConstraint::GoalConstraint(const std::shared_ptr<GoalRegion> &goal)
  : Constraint(goal->getSpaceInformation()->getStateSpace()->getDimension(), 1)
  , goal_(goal)
  , state_(goal->getSpaceInformation()->allocState())
{
}

void GoalConstraint::function(const ompl::base::State *state, Eigen::Ref<Eigen::VectorXd> out) const
{
    out[0] = goal_->distanceGoal(state);
    SE2EZ_DEBUG("GOAL DISTANCE: %1%", out[0]);
}

void GoalConstraint::function(const Eigen::Ref<const Eigen::VectorXd> &x,
                              Eigen::Ref<Eigen::VectorXd> out) const
{
    const auto &space = goal_->getSpaceInformation()->getStateSpace();
    for (unsigned int i = 0; i < space->getDimension(); ++i)
        *space->getValueAddressAtIndex(state_, i) = x[i];

    function(state_, out);
}

double GoalConstraint::distance(const ompl::base::State *state) const
{
    return goal_->distanceGoal(state);
}

bool GoalConstraint::isSatisfied(const ompl::base::State *state) const
{
    return goal_->isSatisfied(state);
}

///
/// Foliation
///

Foliation::Foliation(const unsigned int ambientDim, const unsigned int coDim, const unsigned int leafDim,
                     double tolerance)
  : Constraint(ambientDim, coDim, tolerance), p_(leafDim), t_(coDim - leafDim)
{
}


unsigned int Foliation::getLeafDimension() const
{
    return p_;
}

unsigned int Foliation::getTransverseDimension() const
{
    return t_;
}

void Foliation::setLeafDimension(unsigned int p)
{
    p_ = p;
}

void Foliation::setTransverseDimension(unsigned int t)
{
    t_ = t;
}

FoliationIntersection::FoliationIntersection(const unsigned int ambientDim,
                                             std::vector<FoliationPtr> foliations, double tolerance)
  : Foliation(ambientDim, 0, ambientDim, tolerance)
{
    for (const auto &foliation : foliations)
        addConstraint(foliation);
}

FoliationPtr FoliationIntersection::copy() const 
{
    std::vector<FoliationPtr> foliations;
    for (const auto &foliation : foliations_)
        foliations.emplace_back(foliation->copy());

    return std::make_shared<FoliationIntersection>(n_, foliations, tolerance_);
}

void FoliationIntersection::function(const Eigen::Ref<const Eigen::VectorXd> &x,
                                     Eigen::Ref<Eigen::VectorXd> out) const
{
    unsigned int i = 0;
    for (const auto &foliation : foliations_)
    {
        foliation->function(x, out.segment(i, foliation->getCoDimension()));
        i += foliation->getCoDimension();
    }
}

void FoliationIntersection::jacobian(const Eigen::Ref<const Eigen::VectorXd> &x,
                                     Eigen::Ref<Eigen::MatrixXd> out) const
{
    unsigned int i = 0;
    for (const auto &foliation : foliations_)
    {
        foliation->jacobian(x, out.block(i, 0, foliation->getCoDimension(), n_));
        i += foliation->getCoDimension();
    }
}

Eigen::VectorXd FoliationIntersection::getTransversal(const Eigen::Ref<const Eigen::VectorXd> &state) const
{
    Eigen::VectorXd transverse(t_);

    unsigned int i = 0;
    for (const auto &foliation : foliations_)
    {
        transverse.segment(i, foliation->getTransverseDimension()) = foliation->getTransversal(state);
        i += foliation->getTransverseDimension();
    }

    return transverse;
}

ConstraintPtr FoliationIntersection::getLeaf(const Eigen::Ref<const Eigen::VectorXd> &tv) const
{
    std::vector<ConstraintPtr> constraints;

    unsigned int i = 0;
    for (const auto &foliation : foliations_)
    {
        constraints.emplace_back(foliation->getLeaf(tv.segment(i, foliation->getTransverseDimension())));
        i += foliation->getTransverseDimension();
    }

    return std::make_shared<ConstraintIntersection>(n_, constraints);
}

FoliationPtr FoliationIntersection::getFrond(const Eigen::Ref<const Eigen::VectorXd> &a,
                                             const Eigen::Ref<const Eigen::VectorXd> &b) const
{
    std::vector<FoliationPtr> constraints;

    unsigned int i = 0;
    for (const auto &foliation : foliations_)
    {
        constraints.emplace_back(foliation->getFrond(a.segment(i, foliation->getTransverseDimension()),
                                                     b.segment(i, foliation->getTransverseDimension())));
        i += foliation->getTransverseDimension();
    }

    return std::make_shared<FoliationIntersection>(n_, constraints, tolerance_);
}

void FoliationIntersection::addConstraint(const FoliationPtr &constraint)
{
    setManifoldDimension(k_ - constraint->getCoDimension());
    setLeafDimension(p_ - constraint->getLeafDimension());
    setTransverseDimension(t_ + constraint->getLeafDimension());

    foliations_.push_back(constraint);
}

///
/// ConstraintGoal
///

ConstraintGoal::ConstraintGoal(const ConstraintPtr &constraint, SpaceInformationPtr si, double threshold)
  : GoalLazySamples(
        si, std::bind(&ConstraintGoal::sample, this, std::placeholders::_1, std::placeholders::_2), false)
  , isConstrained_(std::dynamic_pointer_cast<ConstrainedSpaceInformation>(si))
  , isFoliation_(std::dynamic_pointer_cast<FoliationPtr>(constraint))
  , constraint_(allocateConstraint(constraint))
  , sampler_(si_->allocStateSampler())
{
    setThreshold(threshold);

    if (isFoliation_)
    {
        transverse_.reset(new NearestNeighborsGNAT<Transition>());
        transverse_->setDistanceFunction(
            [](const Transition &a, const Transition &b) { return (a.first - b.first).norm(); });

        setNewStateCallback(std::bind(&ConstraintGoal::update, this, std::placeholders::_1));
    }
}

ConstraintGoal::~ConstraintGoal()
{
    stopSampling();
}

ConstraintPtr ConstraintGoal::allocateConstraint(const ConstraintPtr &constraint) const
{
    if (isConstrained_)
    {
        const auto &css = std::static_pointer_cast<ConstrainedStateSpace>(si_->getStateSpace());
        return std::make_shared<ConstraintIntersection>(
            si_->getStateDimension(),
            (std::initializer_list<ConstraintPtr>){css->getConstraint(), constraint});
    }
    else
        return constraint;
}

const Eigen::Ref<const Eigen::VectorXd> ConstraintGoal::getVectorConst(const State *state) const
{
    const auto &ss = si_->getStateSpace();

    // Dynamically set the vector reference to the underlying state memory
    if (isConstrained_)
        return *state->as<ConstrainedStateSpace::StateType>();
    else
        return Eigen::Map<const Eigen::VectorXd>(ss->getValueAddressAtIndex(state, 0), ss->getDimension());
}

Eigen::Ref<Eigen::VectorXd> ConstraintGoal::getVector(State *state) const
{
    const auto &ss = si_->getStateSpace();

    // Dynamically set the vector reference to the underlying state memory
    if (isConstrained_)
        return *state->as<ConstrainedStateSpace::StateType>();
    else
        return Eigen::Map<Eigen::VectorXd>(ss->getValueAddressAtIndex(state, 0), ss->getDimension());
}

double ConstraintGoal::distanceGoal(const State *state) const
{
    return constraint_->distance(getVectorConst(state));
}

bool ConstraintGoal::sample(const GoalLazySamples *, State *state)
{
    Eigen::Ref<Eigen::VectorXd> cs = getVector(state);

    bool success = false;

    unsigned int i = tries_;
    while (not success && i > 0)
    {
        sampler_->sampleUniform(state);
        success = constraint_->project(cs);

        i--;
    }

    return maxSampleCount() < maxSample_;
}

void ConstraintGoal::update(const State *state)
{
    const auto &foliation = std::static_pointer_cast<Foliation>(constraint_);
    Eigen::VectorXd transverse = foliation->getTransversal(getVectorConst(state));

    transverse_->add(std::make_pair(transverse, state));
}