helper.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/rrt/LazyRRT.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/est/EST.h>
#include <ompl/geometric/planners/est/BiEST.h>
#include <ompl/geometric/planners/est/ProjEST.h>
#include <ompl/geometric/planners/sbl/SBL.h>
#include <ompl/geometric/planners/bitstar/BITstar.h>
#include <ompl/geometric/planners/prm/PRM.h>
#include <ompl/geometric/planners/prm/PRMstar.h>
#include <ompl/geometric/planners/prm/LazyPRM.h>
#include <ompl/geometric/planners/prm/LazyPRMstar.h>
#include <ompl/geometric/planners/prm/SPARS.h>
#include <ompl/geometric/planners/prm/SPARStwo.h>
#include <ompl/geometric/planners/fmt/FMT.h>
#include <ompl/geometric/planners/fmt/BFMT.h>
#include <ompl/geometric/planners/pdst/PDST.h>
#include <ompl/geometric/planners/kpiece/KPIECE1.h>
#include <ompl/geometric/planners/kpiece/BKPIECE1.h>
#include <ompl/geometric/planners/kpiece/LBKPIECE1.h>

#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
#include <ompl/base/spaces/constraint/AtlasStateSpace.h>
#include <ompl/base/spaces/constraint/TangentBundleStateSpace.h>

#include <se2ez/core/log.h>
#include <se2ez/core/state.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/box2d/collision.h>

#include <se2ez/plan/space.h>
#include <se2ez/plan/validity.h>
#include <se2ez/plan/helper.h>

using namespace se2ez;

///
/// EZPlans
///

plan::EZPlans::EZPlans(const RobotPtr &robot) : robot_(robot)
{
}

plan::EZPlans::~EZPlans()
{
}

void plan::EZPlans::initialize()
{
    space = rspace = std::make_shared<plan::StateSpace>(robot_);
    setup = std::make_shared<ompl::geometric::SimpleSetup>(space);
    info = setup->getSpaceInformation();

    updateValidityChecker();
}
void plan::EZPlans::updateValidityChecker()
{
    setup->setStateValidityChecker(  //
        std::make_shared<plan::ValidityChecker>(info, std::make_shared<Box2DCollisionManager>(robot_)));
}

void plan::EZPlans::createPlanners()
{
    planners_.emplace("rrt:RRT", makePlanner<ompl::geometric::RRT>());
    planners_.emplace("rrt:RRTConnect", makePlanner<ompl::geometric::RRTConnect>());
    planners_.emplace("rrt:RRT*", makePlanner<ompl::geometric::RRTstar>());
    planners_.emplace("rrt:LazyRRT", makePlanner<ompl::geometric::LazyRRT>());

    planners_.emplace("est:EST", makePlanner<ompl::geometric::EST>());
    planners_.emplace("est:BiEST", makePlanner<ompl::geometric::BiEST>());
    planners_.emplace("est:ProjEST", makePlanner<ompl::geometric::ProjEST>());

    planners_.emplace("sbl:SBL", makePlanner<ompl::geometric::SBL>());

    planners_.emplace("bit:BIT*", makePlanner<ompl::geometric::BITstar>());

    planners_.emplace("prm:PRM", makePlanner<ompl::geometric::PRM>());
    planners_.emplace("prm:PRM*", makePlanner<ompl::geometric::PRMstar>());
    planners_.emplace("prm:LazyPRM", makePlanner<ompl::geometric::LazyPRM>());
    planners_.emplace("prm:LazyPRM*", makePlanner<ompl::geometric::LazyPRMstar>());
    planners_.emplace("spars:SPARS", makePlanner<ompl::geometric::SPARS>());
    planners_.emplace("spars:SPARS2", makePlanner<ompl::geometric::SPARStwo>());

    planners_.emplace("fmt:FMT", makePlanner<ompl::geometric::FMT>());
    planners_.emplace("fmt:BFMT", makePlanner<ompl::geometric::BFMT>());

    planners_.emplace("pdst:PDST", makePlanner<ompl::geometric::PDST>());

    planners_.emplace("kpiece:KPIECE", makePlanner<ompl::geometric::KPIECE1>());
    planners_.emplace("kpiece:BKPIECE", makePlanner<ompl::geometric::BKPIECE1>());
    planners_.emplace("kpiece:LBKPIECE", makePlanner<ompl::geometric::LBKPIECE1>());
}

///
/// EZPlans State Functions
///

bool plan::EZPlans::setStart(const std::string &name)
{
    StatePtr state = std::make_shared<State>(robot_);
    robot_->getNamedState(name, state);

    return setStart(state);
}

bool plan::EZPlans::setGoal(const std::string &name)
{
    StatePtr state = std::make_shared<State>(robot_);
    robot_->getNamedState(name, state);

    return setGoal(state);
}

bool plan::EZPlans::setStartGoal(const std::string &start, const std::string &goal)
{
    return setStart(start) && setGoal(goal);
}

bool plan::EZPlans::setStart(const Eigen::VectorXd &vec)
{
    StatePtr state = std::make_shared<State>(robot_);
    state->data = vec;

    return setStart(state);
}

bool plan::EZPlans::setGoal(const Eigen::VectorXd &vec)
{
    StatePtr state = std::make_shared<State>(robot_);
    state->data = vec;

    return setGoal(state);
}

bool plan::EZPlans::setStartGoal(const Eigen::VectorXd &start, const Eigen::VectorXd &goal)
{
    return setStart(start) && setGoal(goal);
}

bool plan::EZPlans::setStart(const StatePtr &state)
{
    ompl::base::ScopedState<> start(space);
    getState(start.get())->copy(state);
    setup->setStartState(start);

    return postSetState(start.get());
}

bool plan::EZPlans::setGoal(const StatePtr &state)
{
    ompl::base::ScopedState<> goal(space);
    getState(goal.get())->copy(state);
    setup->setGoalState(goal);

    return postSetState(goal.get());
}

bool plan::EZPlans::setStartGoal(const StatePtr &start, const StatePtr &goal)
{
    return setStart(start) && setGoal(goal);
}

bool plan::EZPlans::postSetState(const ompl::base::State * /* state */)
{
    return true;
}

///
/// EZPlans Path Functions
///

std::vector<std::string> plan::EZPlans::getPlanners()
{
    std::vector<std::string> planners;
    for (const auto &planner : planners_)
        planners.emplace_back(planner.first);

    return planners;
}

void plan::EZPlans::setPlanner(const std::string &name)
{
    if (planners_.empty())
        createPlanners();

    setup->setPlanner(planners_[name]);
}

void plan::EZPlans::postprocess(bool simplify, bool interpolate)
{
    if (simplify)
        setup->simplifySolution();

    if (interpolate)
    {
        auto &path = setup->getSolutionPath();
        path.interpolate();
    }
}

std::vector<StatePtr> plan::EZPlans::extractPath() const
{
    auto &path = setup->getSolutionPath();

    std::vector<StatePtr> states;
    for (unsigned int i = 0; i < path.getStateCount(); ++i)
        states.emplace_back(getState(path.getState(i)));

    return states;
}

StatePtr plan::EZPlans::getState(ompl::base::State *state) const
{
    return state->as<plan::StateSpace::StateType>()->state;
}

const StatePtr plan::EZPlans::getStateConst(const ompl::base::State *state) const
{
    return state->as<plan::StateSpace::StateType>()->state;
}

///
/// EZPlansConstraint
///

plan::EZPlansConstraint::EZPlansConstraint(const RobotPtr &robot, const ompl::base::ConstraintPtr &constraint,
                                           ConstraintType method)
  : EZPlans(robot), constraint(constraint), method(method)
{
}

plan::EZPlansConstraint::~EZPlansConstraint()
{
}

void plan::EZPlansConstraint::initialize()
{
    rspace = std::make_shared<plan::StateSpace>(robot_);

    switch (method)
    {
        case PROJECTION:
            space = std::make_shared<ompl::base::ProjectedStateSpace>(rspace, constraint);
            info = std::make_shared<ompl::base::ConstrainedSpaceInformation>(space);
            break;
        case ATLAS:
            space = std::make_shared<ompl::base::AtlasStateSpace>(rspace, constraint);
            info = std::make_shared<ompl::base::ConstrainedSpaceInformation>(space);
            break;
        case TANGENT:
            space = std::make_shared<ompl::base::TangentBundleStateSpace>(rspace, constraint);
            info = std::make_shared<ompl::base::TangentBundleSpaceInformation>(space);
            break;
        default:
            SE2EZ_ERROR("Invalid Space Type!");
            throw std::runtime_error("Invalid Space Type!");
    }

    setup = std::make_shared<ompl::geometric::SimpleSetup>(info);

    updateValidityChecker();
}

void plan::EZPlansConstraint::updateValidityChecker()
{
    setup->setStateValidityChecker(  //
        std::make_shared<plan::WrappedValidityChecker>(info,
                                                       std::make_shared<Box2DCollisionManager>(robot_)));
}

bool plan::EZPlansConstraint::postSetState(const ompl::base::State *state)
{
    if (!checkConstraint(state))
        return false;

    anchor(state);
    return true;
}

StatePtr plan::EZPlansConstraint::getState(ompl::base::State *state) const
{
    return state->as<ompl::base::WrapperStateSpace::StateType>()
        ->getState()
        ->as<plan::StateSpace::StateType>()
        ->state;
}

const StatePtr plan::EZPlansConstraint::getStateConst(const ompl::base::State *state) const
{
    return state->as<ompl::base::WrapperStateSpace::StateType>()
        ->getState()
        ->as<plan::StateSpace::StateType>()
        ->state;
}

void plan::EZPlansConstraint::anchor(const ompl::base::State *state)
{
    if (method == ATLAS || method == TANGENT)
        space->as<ompl::base::AtlasStateSpace>()->anchorChart(state);
}

bool plan::EZPlansConstraint::checkConstraint(const ompl::base::State *state) const
{
    if (!constraint->isSatisfied(state))
    {
        Eigen::VectorXd f(constraint->getCoDimension());
        constraint->function(state, f);

        SE2EZ_WARN("Constraint is not satisfied (%2%) at state %1%",
                   tf::printVector(getStateConst(state)->data), tf::printVector(f));

        return false;
    }

    return true;
}