state.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_CORE_STATE_
#define SE2EZ_CORE_STATE_

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

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

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

    /** \class se2ez::StatePtr
        \brief A shared pointer wrapper for se2ez::State. */

    /** \class se2ez::StateConstPtr
        \brief A const shared pointer wrapper for se2ez::State. */

    /** \brief A representation of a state of a Robot.
     */
    class State
    {
    public:
        SE2EZ_EIGEN_CLASS

        /** \name Constructors
            \{ */

        /** \brief Constructor.
         *  \param[in] robot Robot that this is a state for.
         */
        State(const Robot &robot);

        /** \brief Constructor.
         *  \param[in] robot Robot that this is a state for.
         */
        State(const RobotPtr &robot);

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

        /** \} */

        /** \name Configuration Management
            \{ */

        /** \brief Sets the value of a frame with one joint parameter.
         *  \param[in] robot Robot this state is for.
         *  \param[in] name Name of frame to set value of.
         *  \param[in] value Value to set.
         */
        void setJoint(const RobotPtr &robot, const std::string &name, double value);

        /** \brief Sets the value of a frame with a translation joint.
         *  \param[in] robot Robot this state is for.
         *  \param[in] name Name of frame to set value of.
         *  \param[in] value Value to set.
         */
        void setTranslateJoint(const RobotPtr &robot, const std::string &name, const Eigen::Vector2d &value);

        /** \brief Sets the value of a frame with a floating joint.
         *  \param[in] robot Robot this state is for.
         *  \param[in] name Name of frame to set value of.
         *  \param[in] value Value to set.
         */
        void setFloatJoint(const RobotPtr &robot, const std::string &name, const Eigen::Isometry2d &value);

        /** \brief Sets the value of a frame with a floating joint.
         *  \param[in] robot Robot this state is for.
         *  \param[in] name Name of frame to set value of.
         *  \param[in] value Value to set.
         */
        void setFloatJoint(const RobotPtr &robot, const std::string &name, const Eigen::Vector3d &value);

        /** \brief Sets a random value to the state
         *  \param[in] robot Robot this state is for.
         */
        void setRandom(const RobotPtr &robot);

        /** \brief Copies the configuration from another state.
         *  \param[in] other State to copy.
         */
        void copy(const StatePtr &other);

        /** \} */

        /** \name Pose Management
            \{ */

        /** \brief Compute the forward kinematics for all frames in the robot.
         *  \param[in] robot Robot this state is for.
         */
        void fk(const RobotPtr &robot);

        /** \brief Get the pose of a frame of the robot. fk() must be called before calling this function.
         *  \param[in] robot Robot this state is for.
         *  \param[in] frame Frame to get pose for.
         *  \return Pose of the frame.
         */
        const Eigen::Isometry2d &getPose(const RobotPtr &robot, const std::string &frame);

        /** \brief Get the jacobian of a frame of the robot.
         *  \param[in] robot Robot this state is for.
         *  \param[in] frame Frame to get jacobian for.
         *  \return Jacobian of the frame.
         */
        const Eigen::MatrixXd &getJacobian(const RobotPtr &robot, const std::string &frame);

        /** \brief Get the poses of geometry attached to a frame of the robot. fk() must be called before
         * calling this function.
         *  \param[in] robot Robot this state is for.
         *  \param[in] frame Frame to get pose for.
         *  \return Poses of the frame's geometry.
         */
        const tf::EigenVector<Eigen::Isometry2d> &getGeometryPoses(const RobotPtr &robot,
                                                                   const std::string &frame);

        /** \} */

        /** \name Informative
            \{ */

        /** \brief Print out the current value of all frames on the robot.
         *  \param[in] robot Robot this state is for.
         *  \return Tree of all frames with their poses.
         */
        std::string printFrames(const RobotPtr &robot) const;

        /** \} */

        bool dirty{true};
        Eigen::VectorXd data;         ///< Configuration vector.
        FrameMapPtr frames{nullptr};  ///< Frame poses
    };
}  // namespace se2ez

#endif