示例#1
0
void ompl::base::SpaceInformation::setStateValidityChecker(const StateValidityCheckerFn &svc)
{
    class BoostFnStateValidityChecker : public StateValidityChecker
    {
    public:

        BoostFnStateValidityChecker(SpaceInformation* si,
                                    const StateValidityCheckerFn &fn) : StateValidityChecker(si), fn_(fn)
        {
        }

        virtual bool isValid(const State *state) const
        {
            return fn_(state);
        }

    protected:

        StateValidityCheckerFn fn_;
    };

    if (!svc)
        throw Exception("Invalid function definition for state validity checking");

    setStateValidityChecker(StateValidityCheckerPtr(dynamic_cast<StateValidityChecker*>(new BoostFnStateValidityChecker(this, svc))));
}
示例#2
0
void oc::LTLSpaceInformation::extendValidityChecker(const oc::SpaceInformationPtr& oldsi)
{
    class LTLStateValidityChecker : public ob::StateValidityChecker
    {
    public:
        LTLStateValidityChecker(oc::LTLSpaceInformation* ltlsi,
                                const oc::ProductGraphPtr& prod,
                                const ob::StateValidityCheckerPtr& lowChecker)
            : ob::StateValidityChecker(ltlsi), prod_(prod), lowChecker_(lowChecker), ltlsi_(ltlsi)
        {
        }
        virtual ~LTLStateValidityChecker() { }
        virtual bool isValid(const ob::State* s) const
        {
            return ltlsi_->getProdGraphState(s)->isValid()
                && lowChecker_->isValid(ltlsi_->getLowLevelState(s));
        }
    private:
        const oc::ProductGraphPtr prod_;
        const ob::StateValidityCheckerPtr lowChecker_;
        oc::LTLSpaceInformation* ltlsi_;
    };

    // Some compilers have trouble with LTLStateValidityChecker being hidden in this function,
    // and so we explicitly cast it to its base type.
    setStateValidityChecker(ob::StateValidityCheckerPtr(static_cast<ob::StateValidityChecker*>(
        new LTLStateValidityChecker(this, prod_, oldsi->getStateValidityChecker()))));
}
示例#3
0
CSpaceOMPLSpaceInformation::CSpaceOMPLSpaceInformation(CSpace* _space)
  :ob::SpaceInformation(ob::StateSpacePtr(new CSpaceOMPLStateSpace(_space,this))),cspace(_space)
{
  setStateValidityChecker(ob::StateValidityCheckerPtr(new CSpaceOMPLValidityChecker(this)));
  setMotionValidator(ob::MotionValidatorPtr(new CSpaceOMPLMotionValidator(this)));

  setup();
}
void DensoSetup::initialize(RobotBasePtr probot)
{
    const ob::StateSpacePtr &sSpace = getStateSpace();
    sSpace->setup();

    ob::ScopedState<> start(sSpace);
    ob::ScopedState<> goal(sSpace);
    std::vector<double> startVec(sSpace->getDimension(), 0.);
    std::vector<double> goalVec(sSpace->getDimension(), 0.);
    
    unsigned int ndof = si_->getStateSpace()->as<DensoStateSpace>()->
	_probot->GetActiveDOF();
    for (unsigned int i = 0; i < ndof; i++)
	goalVec[i] = 1.0;

    if (_useRealScene)
    {
	startVec[0] = -1.576;
	startVec[1] = 1.374;
	startVec[2] = 0.0;
	startVec[3] = 0.0;
	startVec[4] = -1.36751533;
	startVec[5] = 1.615;
	goalVec[0] = 1.55;
	goalVec[1] = 1.35;
	goalVec[2] = 0.1;
	goalVec[3] = -000057;
	goalVec[4] = -1.40754;
	goalVec[5] = 1.6;
    }
    
    sSpace->copyFromReals(start.get(), startVec);
    sSpace->copyFromReals(goal.get(), goalVec);
    /* set start, goal, and threshold*/
    setStartAndGoalStates(start, goal, threshold);
    /* set propagation step size and min-max propagation steps*/
    si_->setPropagationStepSize(propagationStepSize);
    si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
    /* initialize a new KPIECE planner */
    setPlanner(ob::PlannerPtr(new oc::KPIECE1(si_)));
    /* set default parameters */
    getPlanner()->as<oc::KPIECE1>()->setMaxCloseSamplesCount(maxCloseSamplesCount);
    getPlanner()->as<oc::KPIECE1>()->setGoalBias(goalBias);
    /* set a state validity checker */
    setStateValidityChecker(ob::StateValidityCheckerPtr
			    (new DensoStateValidityChecker(si_)));
    si_->setStateValidityCheckingResolution(0.05); //5%
    /* set a state propagator */
    setStatePropagator(oc::StatePropagatorPtr(new DensoStatePropagator(si_)));
}
示例#5
0
void KoulesSetup::initialize(unsigned int numKoules, const std::string& plannerName,
    const std::vector<double>& stateVec)
{
    const ob::StateSpacePtr& space = getStateSpace();
    space->setup();
    // setup start state
    ob::ScopedState<> start(space);
    if (stateVec.size() == space->getDimension())
        space->copyFromReals(start.get(), stateVec);
    else
    {
        // Pick koule positions evenly radially distributed, but at a linearly
        // increasing distance from the center. The ship's initial position is
        // at the center. Initial velocities are 0.
        std::vector<double> startVec(space->getDimension(), 0.);
        double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules;
        startVec[0] = .5 * sideLength;
        startVec[1] = .5 * sideLength;
        startVec[4] = .5 * delta;
        for (unsigned int i = 0; i < numKoules; ++i, theta += delta)
        {
            r = .1 + i * .1 / numKoules;
            startVec[4 * i + 5] = .5 * sideLength + r * cos(theta);
            startVec[4 * i + 6] = .5 * sideLength + r * sin(theta);
        }
        space->copyFromReals(start.get(), startVec);
    }
    setStartState(start);
    // set goal
    setGoal(std::make_shared<KoulesGoal>(si_));
    // set propagation step size
    si_->setPropagationStepSize(propagationStepSize);
    // set min/max propagation steps
    si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
    // set directed control sampler; when using the PDST planner, propagate as long as possible
    bool isPDST = plannerName == "pdst";
    const ompl::base::GoalPtr& goal = getGoal();
    si_->setDirectedControlSamplerAllocator(
        [&goal, isPDST](const ompl::control::SpaceInformation *si)
        {
            return KoulesDirectedControlSamplerAllocator(si,  goal, isPDST);
        });
    // set planner
    setPlanner(getConfiguredPlannerInstance(plannerName));
    // set validity checker
    setStateValidityChecker(std::make_shared<KoulesStateValidityChecker>(si_));
    // set state propagator
    setStatePropagator(std::make_shared<KoulesStatePropagator>(si_));
}
void SuperBotSetup::initialize(RobotBasePtr probot)
{
    const ob::StateSpacePtr &sSpace = getStateSpace();
    sSpace->setup();

    ob::ScopedState<> start(sSpace);
    ob::ScopedState<> goal(sSpace);
    std::vector<double> startVec(sSpace->getDimension(), 0.);
    std::vector<double> goalVec(sSpace->getDimension(), 0.);
    
    unsigned int ndof = si_->getStateSpace()->as<SuperBotStateSpace>()->_probot->GetActiveDOF();
    for (unsigned int i = 0; i < ndof; i++)
	goalVec[i] = 1.0;
    
    sSpace->copyFromReals(start.get(), startVec);
    sSpace->copyFromReals(goal.get(), goalVec);
    // set start, goal, and threshold
    setStartAndGoalStates(start, goal, threshold);
    
    // set propagation step size and min-max propagation steps
    si_->setPropagationStepSize(propagationStepSize);
    si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
    
    // initialize a new KPIECE planner
    setPlanner(ob::PlannerPtr(new oc::KPIECE1(si_)));
    
    // set default parameters
    getPlanner()->as<oc::KPIECE1>()->setMaxCloseSamplesCount(maxCloseSamplesCount);
    getPlanner()->as<oc::KPIECE1>()->setGoalBias(goalBias);
    
    // set a state validity checker
    setStateValidityChecker(ob::StateValidityCheckerPtr(new SuperBotStateValidityChecker(si_)));
    si_->setStateValidityCheckingResolution(0.05); //5%
    
    // set a state propagator
    setStatePropagator(oc::StatePropagatorPtr(new SuperBotStatePropagator(si_)));
}
示例#7
0
void ompl::base::SpaceInformation::setStateValidityChecker(const StateValidityCheckerFn &svc)
{
    class FnStateValidityChecker : public StateValidityChecker
    {
    public:
        FnStateValidityChecker(SpaceInformation *si, StateValidityCheckerFn fn)
          : StateValidityChecker(si), fn_(std::move(fn))
        {
        }

        bool isValid(const State *state) const override
        {
            return fn_(state);
        }

    protected:
        StateValidityCheckerFn fn_;
    };

    if (!svc)
        throw Exception("Invalid function definition for state validity checking");

    setStateValidityChecker(std::make_shared<FnStateValidityChecker>(this, svc));
}