motionbuilder.h

Go to the documentation of this file.

/* Author: Constantinos Chamzas  */
#ifndef SE2EZ_EXPERIENCE_MOTIONBUILDER_
#define SE2EZ_EXPERIENCE_MOTIONBUILDER_

#include <set>
#include <map>
#include <se2ez/core/class_forward.h>

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

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

    /** \class se2ez::MotionBuilderPtr
        \brief A shared pointer wrapper for se2ez::MotionBuilder. */

    /** \class se2ez::MotionBuilderConstPtr
        \brief A const shared pointer wrapper for se2ez::MotionBuilder. */

    /** \brief Given a robot it generates queries (IK solutions for a specific set of frames)
     */
    class MotionBuilder
    {
    public:
        SE2EZ_EIGEN_CLASS

        /** \brief Type of the requested goal generation query.
         */
        enum QueryType
        {
            NAMED,  ///< genGoals() returns only Named
            FRAME,  ///< genGoals() returns only states found with IK
            ALL     ///< genGoals() returns both of the above
        };

        /** \name Constructors
            \{ */

        /** \brief Constructor. Will generate queries for these samples
         *  \param[in] robot Robot to generate the queries for
         *  \param[in] ee End-effector to generate the queries for
         *  \param[in] frames Names of frames to find ik solutions
         */
        MotionBuilder(const RobotPtr &robot, const std::string &ee, const std::vector<std::string> &frames);

        /** \brief Constructor.
         *  \param[in] robot Robot to generate the queries from
         *  \param[in] ee End-effector to generate the queries for
         *  \param[in] prefix prefixes of frames (e.g., to find ik solutions)
         */
        MotionBuilder(const RobotPtr &robot, const std::string &ee, const std::string &prefix);

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

        /** \} */

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

        /** \brief generates the goals
         *  \return A map of the frames used for the EE and the ik solutions (goals).
         */
        const std::map<std::string, StatePtr> &genGoals(QueryType type);

        /** \brief Gets the motion planning queries as (Start, Goal) pairs.
         *  \return set of pairs of states.
         */
        const std::set<std::pair<StatePtr, StatePtr>> &getQueries(QueryType type = ALL);

        /** \} */

    private:
        /** \brief Generates goals by finding IK solutions of the ee on the given frames.
         */
        void genFrameGoals();

        /** \brief Generates goals from the named states.
         */
        void genNamedGoals();

        /** \brief Generates both Frame and Named Goals.
         */
        void genAllGoals();

        // TODO: Only chain end effectors are suported for now
        std::map<std::string, StatePtr> goals_;            ///< Valid goals
        std::set<std::pair<StatePtr, StatePtr>> queries_;  ///< valid motion planning queries (Start,Goal)
        std::set<std::string> ees_;                        ///< eeposes  to place the end effector

        RobotPtr robot_;               ///< underlying robot to find configurations  for
        CollisionAwareChainIKPtr ik_;  ///< the ik_solver
        CollisionManagerPtr cm_;       ///< the collision manager to be used by the ik_solver
        const std::string ee_;         ///< end effector used by the ik_solver
    };
}  // namespace se2ez

#endif