frame.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_CORE_FRAME_
#define SE2EZ_CORE_FRAME_

#include <vector>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <kdl/frames.hpp>
#include <kdl/segment.hpp>

#include <se2ez/core/class_forward.h>
#include <se2ez/core/math.h>
#include <se2ez/core/joint.h>

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

    /** \cond IGNORE */
    SE2EZ_CLASS_FORWARD(Frame)
    /** \endcond */

    /** \class se2ez::FramePtr
        \brief A shared pointer wrapper for se2ez::Frame. */

    /** \class se2ez::FrameConstPtr
        \brief A const shared pointer wrapper for se2ez::Frame. */

    /** \brief Representation of a single frame (possibly with a joint) in a kinematic tree.
     */
    class Frame
    {
    public:
        SE2EZ_EIGEN_CLASS

        static FramePtr getRoot();

        /** \name Constructors
            \{ */

        /** \brief Constuctor. Builds a fixed frame at the desired location.
         *  \param[in] name Name of frame.
         *  \param[in] tip Transform to apply after joint transform to reach the tip of the frame.
         *  \param[in] geometry All attached geometry to this frame.
         */
        Frame(const std::string &name, const Eigen::Isometry2d &tip,
              const std::vector<GeometryPtr> &geometry = {});

        /** \brief Constuctor. Builds a fixed frame at the desired location.
         *  \param[in] name Name of frame.
         *  \param[in] tip Transform to apply after joint transform to reach the tip of the frame.
         *  \param[in] geometry All attached geometry to this frame.
         */
        Frame(const std::string &name, const Eigen::Vector3d &tip, const std::vector<GeometryPtr> &geometry);

        /** \brief Constuctor. Builds the segments that compose a frame along with its internal joints.
         *  \param[in] name Name of frame.
         *  \param[in] tip Transform to apply after joint transform to reach the tip of the frame.
         *  \param[in] type Joint type of frame.
         *  \param[in] upper Upper limits of frame joint.
         *  \param[in] lower Lower limits of frame joint.
         *  \param[in] geometry All attached geometry to this frame.
         */
        Frame(const std::string &name, const Eigen::Isometry2d &tip, const Joint::Type &type,
              const Eigen::VectorXd &lower = {}, const Eigen::VectorXd &upper = {},
              const std::vector<GeometryPtr> &geometry = {});

        /** \brief Constuctor. Builds the segments that compose a frame along with its internal joints.
         *  \param[in] name Name of frame.
         *  \param[in] tip Transform to apply after joint transform to reach the tip of the frame.
         *  \param[in] type Joint type of frame.
         *  \param[in] lower Lower limits of frame joint.
         *  \param[in] upper Upper limits of frame joint.
         *  \param[in] geometry All attached geometry to this frame.
         */
        Frame(const std::string &name, const Eigen::Vector3d &tip, const Joint::Type &type,
              const Eigen::VectorXd &lower = {}, const Eigen::VectorXd &upper = {},
              const std::vector<GeometryPtr> &geometry = {});

        /** \brief Constuctor. Builds the segments that compose a frame along with its internal joints.
         *  \param[in] name Name of frame.
         *  \param[in] tip Transform to apply after joint transform to reach the tip of the frame.
         *  \param[in] type Joint type of frame.
         *  \param[in] lower Lower limits of frame joint.
         *  \param[in] upper Upper limits of frame joint.
         *  \param[in] geometry All attached geometry to this frame.
         */
        Frame(const std::string &name, const Eigen::Isometry2d &tip, const Joint::Type &type,
              const double lower, const double upper, const std::vector<GeometryPtr> &geometry = {});

        /** \brief Constuctor. Builds the segments that compose a frame along with its internal joints.
         *  \param[in] name Name of frame.
         *  \param[in] tip Transform to apply after joint transform to reach the tip of the frame.
         *  \param[in] type Joint type of frame.
         *  \param[in] lower Lower limits of frame joint.
         *  \param[in] upper Upper limits of frame joint.
         *  \param[in] geometry All attached geometry to this frame.
         */
        Frame(const std::string &name, const Eigen::Vector3d &tip, const Joint::Type &type,
              const double lower, const double upper, const std::vector<GeometryPtr> &geometry = {});

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

        /** \} */

        /** \name Getters and Setters
            \{ */

        /** \brief Get the name of this frame.
         *  \return The name of the frame.
         */
        const std::string &getName() const;

        /** \brief Get the segments that compose this frame.
         *  \return A vector of the underlying segments.
         */
        const std::vector<KDL::Segment> &getSegments() const;

        /** \brief Get the geometries attached to this frame.
         *  \return A vector of the attached geometries.
         */
        const std::vector<GeometryPtr> &getGeometry() const;

        /** \brief Get the joint within this frame.
         *  \return The joint within the frame.
         */
        const Joint &getJoint() const;

        /** \brief Get the tip of this frame.
         *  \return The tip of this frame.
         */
        const Eigen::Isometry2d &getTip() const;

        /** \} */

        /** \name Geometric and Configuration Operations
            \{ */

        /** \brief Gets the pose of this frame (respect to the root) given a configuration q (for
         * this frame only, e.g., for the floating joint a three dimensional vector)
         *  \param[in] q The configuration for this frame. \return The pose of this frame.
         */
        Eigen::Isometry2d getPose(const Eigen::Ref<const Eigen::VectorXd> &q) const;

        /** \brief Get the poses of each of the attached geometry frames (in order).
         *  \param[in] pose The pose of the frame.
         *  \param[out] poses Geometry poses.
         */
        void getGeometryPoses(const Eigen::Isometry2d &pose, tf::EigenVector<Eigen::Isometry2d> &poses) const;

        /** \} */

        /** \name Informative
            \{ */

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

        /** \} */

    private:
        /** \name Frame Properties
            \{ */

        const std::string name_;       ///< Name of the frame.
        const Eigen::Isometry2d tip_;  ///< The transform after joint transformation to the tip of the frame.
        const Joint joint_;            ///< The joint associated with the frame.

        const std::vector<GeometryPtr> geometry_;   ///< Geometry attached to this frame (at the tip).
        const std::vector<KDL::Segment> segments_;  ///< Underlying segments that compose this frame.

        /* \} */
    };
}  // namespace se2ez

#endif