robot.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_CORE_ROBOT_
#define SE2EZ_CORE_ROBOT_

#include <map>
#include <unordered_map>
#include <vector>
#include <functional>

#include <kdl/tree.hpp>
#include <kdl/treejnttojacsolver.hpp>

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

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

    /** \cond IGNORE */
    SE2EZ_CLASS_FORWARD(Robot)
    SE2EZ_CLASS_FORWARD(FrameMap)
    /** \endcond */

    /** \class se2ez::RobotPtr
        \brief A shared pointer wrapper for se2ez::Robot. */

    /** \class se2ez::RobotConstPtr
        \brief A const shared pointer wrapper for se2ez::Robot. */

    /** \brief Map of frame name to a pair of that describes if the transform is "dirty" (needs updating)
     * and its current transform.
     */
    class FrameMap
    {
    public:
        SE2EZ_EIGEN_CLASS

        /** \brief A tuple that stores whether a transform is dirty (needs to be updated), the transform
         * itself, and the transforms of the geometry associated with the frame.
         */
        struct Value
        {
            SE2EZ_EIGEN_CLASS

            bool dirty{true};
            bool jc{false};
            Eigen::Isometry2d pose{Eigen::Isometry2d::Identity()};
            Eigen::MatrixXd jacobian;
            tf::EigenVector<Eigen::Isometry2d> geometry{};
        };

        /** \name Constructors
            \{ */

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

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

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

        /** \} */

        /** \brief Dirties all frames within the frame map.
         */
        void dirty();

        /** \name Entry Manipulation
            \{ */

        /** \brief Inserts all entries in this frame map into another frame map.
         *  \param[out] other Other frame map to insert entries into.
         */
        void insert(FrameMapPtr other);

        /** \brief Gets the entry in the frame map associated with \e frame_name. Creates the entry if one
         * does not already exist.
         *  \param[in] frame_name Name of frame to get entry for.
         *  \return Entry associated with frame.
         */
        Value &getEntry(const std::string &frame_name);

        /** \brief Gets the entry in the frame map associated with \e frame_name. Throws error if it does
         * not already exist.
         *  \param[in] frame_name Name of frame to get entry for.
         *  \return Entry associated with frame.
         */
        const Value &getEntryConst(const std::string &frame_name) const;

        /** \} */

        /** \brief Prints out all current frame pose information. Attaches "_[ x, y, t ]" to the
         * Robot::printTree(), where '_' is 'C' for clean, 'D' for dirty, and 'X' for uninitialized.
         * "[ x, y, t ]" is the transform of the frame.
         *  \param[in] robot Robot that this frame map is associated with.
         *  \return Printed tree with frame information.
         */
        std::string printFrames(const RobotPtr &robot) const;

    private:
        tf::EigenHash<std::string, Value> frames;  ///< Map of frame name to dirty / transform pairs.
    };

    /** \brief A representation of a robot (in this case, a kinematic tree in the plane).
     */
    class Robot
    {
    public:
        SE2EZ_EIGEN_CLASS

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

        /** \class se2ez::Robot::FrameDataPtr
            \brief A shared pointer wrapper for se2ez::Robot::FrameData. */

        /** \class se2ez::Robot::FrameDataConstPtr
            \brief A const shared pointer wrapper for se2ez::Robot::FrameData. */

        /** \brief All associated data with a frame in a kinematic tree.
         */
        class FrameData
        {
        public:
            SE2EZ_EIGEN_CLASS

            /** \name Constructors
                \{ */

            /** \brief Constructor for the root element.
             */
            FrameData();

            /** \brief Constructor for any other frame of the tree.
             *  \param[in] frame Frame to create frame data for.
             *  \param[in] parent FrameData of parent.
             */
            FrameData(FramePtr frame, FrameDataPtr parent);

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

            /** \} */

            /** \name Configuration Management
                \{ */

            /** \brief Returns a reference to the underlying subvector of the configuration associated with
             * this frame.
             *  \param[in] data Configuration to extract component from.
             *  \return Reference to subvector within configuration associated with the frame's joint values.
             */
            Eigen::Ref<Eigen::VectorXd> getValues(Eigen::Ref<Eigen::VectorXd> data) const;

            /** \brief Returns a reference to the underlying subvector of the configuration associated with
             * this frame.
             *  \param[in] data Configuration to extract component from.
             *  \return Reference to subvector within configuration associated with the frame's joint values.
             */
            const Eigen::Ref<const Eigen::VectorXd>
            getValuesConst(const Eigen::Ref<const Eigen::VectorXd> &data) const;

            /** \} */

            bool root{false};                      ///< Is this frame the root?
            FramePtr frame{nullptr};               ///< Underlying frame.
            std::vector<FrameDataPtr> children{};  ///< Children of this frame.
            FrameDataPtr parent{nullptr};          ///< Parent of this frame.
            unsigned int depth{0};                 ///< Depth of frame in tree.

            unsigned int index{0};  ///< Index within tree's configuration for the frame's joint values.
            unsigned int size{0};   ///< Number of joint values for this frame.

            bool movable{false};  ///< True if this frame has an actuated joint before it.
        };

        /** \name Constructors
            \{ */

        /** \brief Constructor. Nothing interesting right now.
         */
        Robot();

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

        /** \} */

        /** \name Frame / Kinematic Tree Management
            \{ */

        /** \brief Attach a frame to the root of the kinematic tree. Will detach frame if attached
         * elsewhere already. \param[in] frame Frame to attach.
         */
        void attachFrame(const FramePtr &frame);

        /** \brief Attach a frame to another frame in the kinematic tree. Will detach frame if attached
         * elsewhere already.
         *  \param[in] frame Frame to attach.
         *  \param[in] parent Parent to attach frame to (must be in tree already).
         */
        void attachFrame(const FramePtr &frame, const FramePtr &parent);

        /** \brief Attach a frame to another frame in the kinematic tree. Will detach frame if attached
         * elsewhere already.
         *  \param[in] frame Frame to attach.
         *  \param[in] parent_name Name of parent to attach frame to.
         */
        void attachFrame(const FramePtr &frame, const std::string &parent_name);

        /** \brief Detach a frame from a kinematic tree. If frame has children all children will be detached
         * as well.
         *  \param[in] frame Frame to detach
         */
        void detachFrame(const FramePtr &frame);

        /** \brief Detach a frame from a kinematic tree. If frame has children all children will be detached
         * as well.
         *  \param[in] frame_name Frame name to detach.
         */
        void detachFrame(const std::string &frame_name);

        /** \brief add a fixed Robot(frames with all joints fixed) on the current robot.
         * The frame names are namespaced under the scene_name.
         *  \param[in] frobot Robot to add as a fixed scene.
         *  \param[in] state State of the attached Scene;
         *  \param[in] scene_name std::string name of the attached scene
         */
        void addFixedRobot(const RobotPtr &frobot, const StatePtr &state, const std::string &scene_name);

        /** \brief adds an active Robot with all its frames and corresponding joints.
         *  \param[in] arobot Robot to add as the active one.
         */
        void addActiveRobot(const RobotPtr &arobot);

        /** \brief Compiles the underlying kinematic tree representation after a call to attachFrame().
         */
        void compileTree();

        /** \brief Completely clears internal structures, a brand new robot!
         */
        void clear();

        /** \} */

        /** \name Pose / Configuration Management
            \{ */

        /** \brief Compute the forward kinematics of the robot at a state \e q.
         *  \param[in] q Configuration of the robot.
         *  \param[out] frames Forward kinematics.
         */
        void fk(const Eigen::Ref<const Eigen::VectorXd> &q, FrameMapPtr frames) const;

        /** \brief Gets the pose of a frame \e frame_name at a configuration \e q. Also updates all dependent
         * frames in \e frames.
         *  \param[in] frame_name Name of frame to get pose for.
         *  \param[in] q Configuration of robot to compute pose for.
         *  \param[in] frames Workspace for computing frames. Respects dirty frame information.
         *  \return Current pose of the frame.
         */
        const FrameMap::Value &getPose(const std::string &frame_name,
                                       const Eigen::Ref<const Eigen::VectorXd> &q, FrameMapPtr frames) const;

        /** \brief Gets the jacobian for \e frame_name at configuration \e q.
         *  \param[in] frame_name Name of frame to get jacobian for.
         *  \param[in] q Configuration of robot to compute jacobian for.
         *  \param[in] frames Workspace for computing jacobian.
         */
        const FrameMap::Value &getJacobian(const std::string &frame_name,
                                           const Eigen::Ref<const Eigen::VectorXd> &q,
                                           FrameMapPtr frames) const;

        /** \brief Returns a reference to the underlying subvector of the configuration associated with a
         * specific frame.
         *  \param[in] frame_name Name of frame to get component of configuration for.
         *  \param[in] q Configuration to extract component from.
         *  \return Reference to subvector within configuration associated with a frame's joint values.
         */
        Eigen::Ref<Eigen::VectorXd> getFrameValues(const std::string &frame_name,
                                                   Eigen::Ref<Eigen::VectorXd> q) const;

        /** \brief Returns a reference to the underlying subvector of the configuration associated with a
         * specific frame.
         *  \param[in] frame_name Name of frame to get component of configuration for.
         *  \param[in] q Configuration to extract component from.
         *  \return Reference to subvector within configuration associated with a frame's joint values.
         */
        const Eigen::Ref<const Eigen::VectorXd>
        getFrameValuesConst(const std::string &frame_name, const Eigen::Ref<const Eigen::VectorXd> &q) const;

        /** \brief Compute the distance between two states.
         *  \param[in] a One state.
         *  \param[in] b Other state.
         *  \param[in] weighted Weight distance by inverse of depth of frame in tree.
         *  \return The distance between a and b.
         */
        double distance(const StatePtr &a, const StatePtr &b, bool weighted = true) const;

        /** \brief Enforce joint limits on a robot and renormalize angles if necessary.
         *  \param[in, out] a State to enforce limits on.
         */
        void enforceLimits(StatePtr a) const;

        /** \brief Check if all configuration parameters are within limits.
         *  \param[in] a State to check.
         *  \return True if within limits, false otherwise.
         */
        bool inLimits(const StatePtr &a) const;

        /** \brief Interpolate between two states.
         *  \param[in] from State you are interpolating from.
         *  \param[in] to State you are interpolating to.
         *  \param[in] t Interpolation parameter in [0, 1]. 0 is \e from, 1 is \e to.
         *  \param[out] state Interpolated state.
         */
        void interpolate(const StatePtr &from, const StatePtr &to, double t, StatePtr state) const;

        /** \} */

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

        /** \brief Gets the number of controllable joints within the tree.
         */
        unsigned int getNumJoints() const;

        /** \brief Gets the names of end-effectors of the tree (leaf nodes).
         *  \return Vector of all end-effectors in the tree.
         */
        const std::vector<std::string> &getEEs() const;

        /** \brief Get a vector of all the frame names within this robot.
         *  \return A vector of all frame names.
         */
        const std::vector<std::string> &getFrameNames() const;

        /** \brief Get the frame with name \e name.
         *  \param[in] name Name of the frame to get.
         *  \return Frame information.
         */
        FramePtr getFrame(const std::string &name);

        /** \brief Get the parent of frame with name \e name. Throws Error if name is root.
         *  \param[in] name Name of the frame to get.
         *  \return Frame information.
         */
        FramePtr getFrameParent(const std::string &name);

        /** \brief Get the frame with name \e name.
         *  \param[in] name Name of the frame to get.
         *  \return Frame information.
         */
        const FramePtr &getFrameConst(const std::string &name) const;

        /** \brief Checks if a frame \e name is in the tree.
         *  \param[in] name Frame to check.
         *  \return True if frame is in tree, false otherwise.
         */
        bool inTree(const std::string &name) const;

        /** \brief Retrieves kinematic tree data associated with the frame \e frame_name.
         *  \param[in] frame_name Name of frame to get data for.
         *  \return The frame data of the frame.
         *  \throws Exception if frame does not exist.
         */
        const FrameDataPtr &getFrameDataConst(const std::string &frame_name) const;

        /** \brief Returns the signature of the robot
         *  \return the robot signature
         */
        const std::string &getSignature();

        /** \brief Sets a named state.
         *  \param[in] name Name of state to set.
         *  \param[in] state State to set named state to.
         */
        void setNamedState(const std::string &name, const StatePtr &state);

        /** \brief Sets a named state.
         *  \param[in] named Map of named states.
         */
        void setNamedStates(const std::map<std::string, StatePtr> &named);

        /** \brief Gets a named state.
         *  \param[in] name Name of state to get.
         *  \param[out] state State to fill with named state.
         */
        void getNamedState(const std::string &name, StatePtr state);

        /** \brief Gets the names of all named states.
         *  \return List of state names.
         */
        std::vector<std::string> getNamedStates() const;

        /** \brief Gets the names of all named states.
         *  \return map of state names.
         */
        const std::map<std::string, StatePtr> &getNamedStatesMap();

        /** \brief Gets the robot's default ACM.
         *  \return The robot's ACM.
         */
        ACMPtr getACM();

        /** \brief Sets the robot's default ACM.
         *  \param[in] acm The ACM to set.
         */
        void setACM(const ACMPtr &acm);

        /** \brief Get the upper limits of the joints for this robot.
         *  \return The upper limits of the robot.
         */
        const Eigen::VectorXd getUpperLimits() const;

        /** \brief Get the lower limits of the joints for this robot.
         *  \return The lower limits of the robot.
         */
        const Eigen::VectorXd getLowerLimits() const;

        /** \brief Returns a vector of booleans. If an entry is true, that index is a continuous joint.
         *  \return Continuity of joints.
         */
        const std::vector<bool> getContinuity() const;

        /** \brief Get the ordered list of active joints in this robot with their associated frame
         * information. \return The ordered list of active joints.
         */
        const std::vector<std::pair<std::string, FrameDataPtr>> &getJoints() const;

        /** \brief Get the underlying KDL tree for this robot.
         *  \return The KDL tree.
         */
        const KDL::Tree &getTree() const;

        /** \} */

        /** \name Informative
            \{ */

        /** \brief Function used by Robot::printTree() to print additional information about frames.
         */
        using PrintHelper = std::function<std::string(const FrameDataPtr &)>;

        /** \brief Returns a string that graphically shows the kinematic structure of the tree. For
         * example: \verbatim
         * \_ root (output of function goes here)
         *  \_ base
         *   \_ link_1
         *    \_ link_2
         *    |\_ link_3
         *    | \_ e2
         *    \_ e1
         * \endverbatim
         *  \param[in] function A function to be called for each frame, with output attached at end of
         * tree.
         *  \return A string that represents the tree.
         */
        std::string printTree(const PrintHelper &function = {}) const;

        /** \brief Returns a string of complete robot information for debugging purposes.
         *  \return A string of tree information.
         */
        std::string printDebug() const;

        /** \} */

    private:
        /** \name Getters / Setters
            \{ */

        /** \brief Retrieves kinematic tree data associated with the frame \e frame_name.
         *  \param[in] frame_name Name of frame to get data for.
         *  \return The frame data of the frame. Throws exception if frame does not exist.
         */
        FrameDataPtr getFrameData(const std::string &frame_name);

        /** \} */

        /** \name Frame / Kinematic Tree Management
            \{ */

        /** \brief Removes \e child from the \e parent in the tree.
         *  \param[in] parent Parent to remove child from.
         *  \param[in] child Name of child to remove.
         *  \throws Exception if child is not a child of parent.
         */
        void removeChild(FrameDataPtr parent, const std::string &child);

        /** \brief Updates a frame's entry in the kinematic tree with a possibly new parent \e parent_name.
         *  \param[in] frame Frame to update (will also add frame if new).
         *  \param[in] parent_name Name of parent frame in kinematic tree.
         */
        void updateFrame(const FramePtr &frame, const std::string &parent_name);

        /** \brief Helper function to compileTree() that recursively build tree representation.
         *  \param[in] previous Previous frame in the kinematic tree.
         *  \param[in] data Frame to add to the tree.
         *  \param[in] movable Was there a movable joint before this frame?
         */
        void compileTreeRecursive(const std::string &previous, FrameDataPtr &data, bool movable = false);

        /** \brief Compute the transform for all non-movable frames.
         */
        void staticFK();

        /** \brief Creates a unique signature (based on active joints).
         *  \return the robot signature
         */
        void generateSignature();

        /** \} */

        /** \name Pose / Configuration Management
            \{ */

        /** \brief Gets the pose of a frame \e data at a configuration \e q. Also updates all dependent
         * frames in \e frames.
         *  \param[in] data Frame data to get pose for.
         *  \param[in] q Configuration of robot to compute pose for.
         *  \param[in] frames Workspace for computing frames. Respects dirty frame information.
         *  \return Current pose of the frame.
         */
        const FrameMap::Value &getPose(const FrameDataPtr &data, const Eigen::Ref<const Eigen::VectorXd> &q,
                                       FrameMapPtr frames) const;

        /** \} */

        /** \name Informative
            \{ */

        /** \brief Helper function to printTree() that recursively traverses the kinematic tree.
         *  \param[in] ss Stream to output data to.
         *  \param[in] frame Frame to output data about.
         *  \param[in] function User provided function to call for each frame.
         *  \param[in] prefix Prefix to attach to frame output.
         */
        void printTreeRecursive(std::stringstream &ss, const FrameDataPtr &frame, const PrintHelper &function,
                                const std::string &prefix = "") const;

        /** \} */

        std::map<std::string, FrameDataPtr> data_;  ///< Kinematic tree data for all frames.
        std::vector<std::string> ees_;    ///< End-effectors (leaves) in the kinematic tree that are mobile.
        std::vector<std::string> names_;  ///< Names of the the kinematic tree frames (ordered).

        std::vector<std::pair<std::string, FrameDataPtr>> joints_;  ///< Ordered vector of active joints.
        Eigen::VectorXd upper_;                                     ///< Upper limits of joints.
        Eigen::VectorXd lower_;                                     ///< Lower limits of joints.
        unsigned int maxDepth_;                                     ///< Maximum tree depth.

        KDL::Tree tree_;                                         ///< Underlying tree representation.
        std::shared_ptr<KDL::TreeJntToJacSolver> jac_{nullptr};  ///< Underlying tree representation.
        bool tree_update_{true};                                 ///< Does the tree need recompiling?

        std::map<std::string, StatePtr> named_;  ///< Named states.
        std::string signature_;                  ///< A string that represents the current kinematic chain

        ACMPtr acm_;                   ///< Default robot ACM.
        FrameMapPtr static_{nullptr};  ///< Transforms for all non-movable links.
    };
}  // namespace se2ez

#endif