joint.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_CORE_JOINT_
#define SE2EZ_CORE_JOINT_

#include <vector>

#include <Eigen/Core>

#include <kdl/joint.hpp>

#include <se2ez/core/class_forward.h>

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

    /** \class se2ez::JointPtr
        \brief A shared pointer wrapper for se2ez::Joint. */

    /** \class se2ez::JointConstPtr
        \brief A const shared pointer wrapper for se2ez::Joint. */

    /** \brief Container class that stores the underlying joint representation.
     *  Can represent basic joints within the plane, such as fixed (rigid attachment), float (free-flying in
     *  SE(2)), translate (free translation in R^2), prismatic (extension along the X-axis), and revolute
     *  (rotation).
     */
    class Joint
    {
    public:
        SE2EZ_EIGEN_CLASS

        /** \brief Type of joint.
         */
        enum Type
        {
            FIXED = 0,   ///< Fixed joint, just a rigid transformation.
            FLOAT = 1,   ///< Floating joint, free movement in SE(2). Uses Euclidean metric.
            FLYING = 2,  ///< Floating joint, free movement in SE(2). Continuous rotation, uses SE(2) metric.
            TRANSLATE = 3,  ///< Translating joint, free X- and Y-axis movement.
            PRISMATIC = 4,  ///< Prismatic joint, extends along X-axis.
            REVOLUTE = 5,   ///< Revolute joint, rotates. Uses Euclidean metric.
            CONTINUOUS = 6  ///< Revolute joint, rotates. Continuous rotation, uses SO(2) metric.
        };
        /** \brief Maximum value a joint type can have */
        static const unsigned int TYPE_MAX = CONTINUOUS;
        /** \brief String representation of joint type. */
        static const std::string TYPE_STRINGS[];

        /** \brief Try to convert a string into a joint type. Not case-sensitive.
         *  \param[in] joint String of joint type. See TYPE_STRINGS.
         *  \return The type of joint specified by the string, or an exception on failure.
         */
        static Type stringToType(const std::string &joint);

        /** \brief Converts a joint type into its string representation.
         *  \param[in] type The type to convert.
         *  \return The joint type as a string.
         */
        static const std::string &typeToString(const Type &type);

        /** \name Constructors
            \{ */

        /** \brief Constructor. Takes in name of joint (the frame this joint is associated with, normally) and
         *  the joint type to create underlying structures.
         *  \param[in] name Name of the joint.
         *  \param[in] type Type of the joint.
         *  \param[in] lower Lower limits of frame joint.
         *  \param[in] upper Upper limits of frame joint.
         */
        Joint(const std::string &name, Type type, const Eigen::VectorXd &lower, const Eigen::VectorXd &upper);

        // non-copyable
        Joint(const Joint &) = delete;
        Joint(Joint &&) = delete;

        /** \} */

        /** \name Joint-based Operations
            \{ */

        /** \brief Enforces joint limits on a joint configuration.
         *  \param[in] q Configuration for the joint.
         */
        void enforceBounds(Eigen::Ref<Eigen::VectorXd> q) const;

        /** \brief sets the Joint at a random state respecting the limits.
         *  \param[in] q Configuration for the joint.
         */
        void setRandom(Eigen::Ref<Eigen::VectorXd> q) const;

        /** \brief Checks if joint limits are satisfied for a joint configuration.
         *  \param[in] q Configuration for the joint.
         *  \return True if configuration is within bounds.
         */
        bool inBounds(const Eigen::Ref<const Eigen::VectorXd> &q) const;

        /** \brief Measures distance between two joint configurations.
         *  \param[in] a First configuration.
         *  \param[in] b First configuration.
         *  \return Distance between a and b
         */
        double distance(const Eigen::Ref<const Eigen::VectorXd> &a,
                        const Eigen::Ref<const Eigen::VectorXd> &b) const;

        /** \brief Interpolate between two joint configurations.
         *  \param[in] from Initial configuration.
         *  \param[in] to Destination configuration.
         *  \param[in] t Interpolation parameter.
         *  \param[out] state Output configuration.
         *  \return Fills state with interpolated parameters.
         */
        void interpolate(const Eigen::Ref<const Eigen::VectorXd> &from,
                         const Eigen::Ref<const Eigen::VectorXd> &to, double t,
                         Eigen::Ref<Eigen::VectorXd> state) const;

        /** \brief Get the bounds for this joint. For continuous joints, [-pi, pi] is returned.
         *  \return A pair of lower and upper bounds for the joint.
         */
        std::pair<Eigen::VectorXd, Eigen::VectorXd> getBounds() const;

        /** \} */

        /** \name Informative
            \{ */

        /** \brief Print debug information about this frame.
         *  \return A string of information about the frame.
         */
        std::string printDebug() const;

        /** \} */

        const Type type;                       ///< Type of joint.
        const std::vector<KDL::Joint> joints;  ///< Underlying KDL joints.
        const bool continuous;                 ///< Is this a continuous type joint?
        const bool fixed;                      ///< Is this joint fixed?
        const unsigned int n;                  ///< Number of joints.

        const Eigen::VectorXd lower;  ///< Lower limits of this frame's joint value (closed).
        const Eigen::VectorXd upper;  ///< Upper limits of this frame's joint value (closed).

    private:
        const unsigned int top_;  ///< Top index for internal computations.
    };
}  // namespace se2ez

#endif