constraint.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_PLAN_CONSTRAINT_
#define SE2EZ_PLAN_CONSTRAINT_

#include <mutex>
#include <ompl/base/Constraint.h>

#include <se2ez/core/class_forward.h>

#include <se2ez/plan/goals.h>

namespace se2ez
{
    /** \cond IGNORE */
    SE2EZ_CLASS_FORWARD(Robot)
    SE2EZ_CLASS_FORWARD(State)
    /** \endcond */

    namespace plan
    {
        std::map<std::string, ompl::base::ConstraintPtr> loadConstraints(const RobotPtr &robot,
                                                                         const std::string &filename);

        SE2EZ_CLASS_FORWARD(FrameConstraint)
        SE2EZ_CLASS_FORWARD(LineConstraint)
        SE2EZ_CLASS_FORWARD(PointConstraint)

        class FrameConstraint : public ompl::base::Foliation
        {
        public:
            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;

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

            bool project(ompl::base::State *state) const override;
            bool project(Eigen::Ref<Eigen::VectorXd> x) const override;

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

            void setDrawOffset(const Eigen::Ref<const Eigen::Vector2d> &v);
            const Eigen::Vector2d &getDrawOffset() const;

        protected:
            FrameConstraint(const std::string &name, const RobotPtr &robot, const std::string &frame,
                            const std::string &base, unsigned int dimension = 1, unsigned int leaf = 0);

            const std::pair<Eigen::Isometry2d, Eigen::Isometry2d>
            getPose(const ompl::base::State *state) const;

            const std::pair<Eigen::Isometry2d, Eigen::Isometry2d>
            getPose(const Eigen::Ref<const Eigen::VectorXd> &x) const;

            virtual void functionInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                          Eigen::Ref<Eigen::VectorXd> out) const = 0;
            virtual void jacobianInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                          const Eigen::MatrixXd &jac,
                                          Eigen::Ref<Eigen::MatrixXd> out) const = 0;

            Eigen::Vector2d draw_{Eigen::Vector2d::Zero()};
            const std::string name_;
            const RobotPtr robot_;
            const std::string frame_;
            const std::string base_;
            StatePtr state_;
            mutable std::recursive_mutex mutex_;
        };

        class LineConstraint : public FrameConstraint
        {
        public:
            LineConstraint(const std::string &name, const RobotPtr &robot, const std::string &frame,
                           const std::string &base, const Eigen::Vector2d &a, const Eigen::Vector2d &b);

            ompl::base::FoliationPtr copy() const override;
            // PointConstraintPtr getLeaf(const StatePtr &state) const;

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

            void getEndpoints(Eigen::Ref<Eigen::VectorXd> a, Eigen::Ref<Eigen::VectorXd> b,
                              const StatePtr &state) const;

        protected:
            void functionInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                  Eigen::Ref<Eigen::VectorXd> out) const override;
            void jacobianInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                  const Eigen::MatrixXd &jac, Eigen::Ref<Eigen::MatrixXd> out) const override;

        private:
            enum Place
            {
                A,
                LINE,
                B
            };

            struct Result
            {
                Eigen::Vector2d at;
                Eigen::Vector2d bt;
                Eigen::Vector2d vt;

                Eigen::Vector2d ca;
                Eigen::Vector2d cb;
                double cv;

                Place place;
            };

            void getPlace(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base, Result &r) const;

            const Eigen::Vector2d a_;
            const Eigen::Vector2d b_;
            const Eigen::Vector2d v_;
            const double abn_;
        };

        class PointConstraint : public FrameConstraint
        {
        public:
            PointConstraint(const std::string &name, const RobotPtr &robot, const std::string &frame,
                            const std::string &base, const Eigen::Vector2d &p);

            ompl::base::FoliationPtr copy() const override;

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

            void getPoint(Eigen::Ref<Eigen::VectorXd> a, const StatePtr &state) const;

        protected:
            void functionInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                  Eigen::Ref<Eigen::VectorXd> out) const override;
            void jacobianInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                  const Eigen::MatrixXd &jac, Eigen::Ref<Eigen::MatrixXd> out) const override;

        private:
            const Eigen::Vector2d p_;
        };

        class CircleConstraint : public FrameConstraint
        {
        public:
            CircleConstraint(const std::string &name, const RobotPtr &robot, const std::string &frame,
                             const std::string &base, const Eigen::Vector2d &p, const Eigen::Vector2d &r, unsigned int exponent = 2);

            ompl::base::FoliationPtr copy() const override;

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

            void getCircle(Eigen::Ref<Eigen::VectorXd> p, Eigen::Ref<Eigen::VectorXd> r, double &v,
                           const StatePtr &state) const;

        protected:
            double getAngle(const Eigen::Isometry2d &frame, const Eigen::Isometry2d &base) const;
            void functionInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                  Eigen::Ref<Eigen::VectorXd> out) const override;
            void jacobianInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                  const Eigen::MatrixXd &jac, Eigen::Ref<Eigen::MatrixXd> out) const override;

        private:
            const Eigen::Vector2d p_;
            const Eigen::Vector2d r_;
            const unsigned int exp_;
        };

        // class OrientationConstraint : public FrameConstraint
        // {
        // public:
        //     OrientationConstraint(const RobotPtr &robot, const std::string &frame,  //
        //                           const double v);

        // protected:
        //     void functionInternal(const Eigen::Isometry2d &pose,  //
        //                           Eigen::Ref<Eigen::VectorXd> out) const override;
        //     void jacobianInternal(const Eigen::Isometry2d &pose, const Eigen::MatrixXd &jac,  //
        //                           Eigen::Ref<Eigen::MatrixXd> out) const override;

        // private:
        //     const double v_;
        // };

    }  // namespace plan
}  // namespace se2ez

#endif