goals.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_PLAN_GOALS_
#define SE2EZ_PLAN_GOALS_

#include <ompl/base/Constraint.h>
#include <ompl/datastructures/NearestNeighborsGNAT.h>
#include <ompl/base/goals/GoalLazySamples.h>

namespace ompl
{
    namespace base
    {
        OMPL_CLASS_FORWARD(WrapperGoal);
        class WrapperGoalRegion : public GoalRegion
        {
        public:
            WrapperGoalRegion(const SpaceInformationPtr &si, const std::shared_ptr<GoalRegion> &goal);

            bool isSatisfied(const State *state) const override;
            bool isSatisfied(const State *state, double *distance) const override;
            double distanceGoal(const State *state) const override;
            void print(std::ostream &out = std::cout) const override;
            void setThreshold(double threshold);
            double getThreshold() const;

        private:
            const std::shared_ptr<GoalRegion> goal_;
        };

        OMPL_CLASS_FORWARD(GoalConstraint);
        class GoalConstraint : public Constraint
        {
        public:
            GoalConstraint(const std::shared_ptr<GoalRegion> &goal);

            void function(const ompl::base::State *state,  //
                          Eigen::Ref<Eigen::VectorXd> out) const override;
            void function(const Eigen::Ref<const Eigen::VectorXd> &x,  //
                          Eigen::Ref<Eigen::VectorXd> out) const override;

            double distance(const ompl::base::State *state) const override;
            bool isSatisfied(const ompl::base::State *state) const override;

        private:
            const std::shared_ptr<GoalRegion> goal_;
            State *state_;
        };

        OMPL_CLASS_FORWARD(Foliation);
        class Foliation : public Constraint
        {
        public:
            Foliation(const unsigned int ambientDim, const unsigned int coDim, const unsigned int leafDim,
                      double tolerance = magic::CONSTRAINT_PROJECTION_TOLERANCE);

            virtual FoliationPtr copy() const = 0;

            virtual Eigen::VectorXd getTransversal(const Eigen::Ref<const Eigen::VectorXd> &state) const = 0;

            virtual ConstraintPtr getLeaf(const Eigen::Ref<const Eigen::VectorXd> &tv) const = 0;

            virtual FoliationPtr getFrond(const Eigen::Ref<const Eigen::VectorXd> &a,
                                          const Eigen::Ref<const Eigen::VectorXd> &b) const = 0;

            unsigned int getLeafDimension() const;
            unsigned int getTransverseDimension() const;
            void setLeafDimension(unsigned int p);
            void setTransverseDimension(unsigned int t);

        protected:
            unsigned int p_;  ///< Leaf dimension.
            unsigned int t_;  ///< Transverse dimension.
        };

        OMPL_CLASS_FORWARD(FoliationIntersection);
        class FoliationIntersection : public Foliation
        {
        public:
            FoliationIntersection(const unsigned int ambientDim, std::vector<FoliationPtr> foliations,
                                  double tolerance = magic::CONSTRAINT_PROJECTION_TOLERANCE);

            FoliationPtr copy() const override;

            void function(const Eigen::Ref<const Eigen::VectorXd> &x,
                          Eigen::Ref<Eigen::VectorXd> out) const override;

            void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x,
                          Eigen::Ref<Eigen::MatrixXd> out) const override;

            Eigen::VectorXd getTransversal(const Eigen::Ref<const Eigen::VectorXd> &state) const override;
            ConstraintPtr getLeaf(const Eigen::Ref<const Eigen::VectorXd> &tv) const override;
            FoliationPtr getFrond(const Eigen::Ref<const Eigen::VectorXd> &a,
                                  const Eigen::Ref<const Eigen::VectorXd> &b) const override;

        protected:
            void addConstraint(const FoliationPtr &constraint);

            /** \brief Constituent constraints. */
            std::vector<FoliationPtr> foliations_;
        };

        OMPL_CLASS_FORWARD(ConstraintGoal);
        class ConstraintGoal : public GoalLazySamples
        {
        public:
            ConstraintGoal(const ConstraintPtr &constraint, SpaceInformationPtr si, double threshold = 1e-5);
            ~ConstraintGoal() override;

            double distanceGoal(const State *state) const override;

        protected:
            bool sample(const GoalLazySamples *gls, State *state);
            void update(const State *state);

            Eigen::Ref<Eigen::VectorXd> getVector(State *state) const;
            const Eigen::Ref<const Eigen::VectorXd> getVectorConst(const State *state) const;

            const bool isConstrained_;
            const bool isFoliation_;
            const ConstraintPtr constraint_;
            const StateSamplerPtr sampler_;

            using Transition = std::pair<Eigen::VectorXd, const State *>;
            std::shared_ptr<NearestNeighbors<Transition>> transverse_;

        private:
            const unsigned int tries_{5};
            const unsigned int maxSample_{100};
            ConstraintPtr allocateConstraint(const ConstraintPtr &constraint) const;
        };
    }  // namespace base
}  // namespace ompl

#endif