ik.h

Go to the documentation of this file.

/* Author: Zachary Kingston */

#ifndef SE2EZ_CORE_IK_
#define SE2EZ_CORE_IK_

#include <mutex>

#include <kdl/tree.hpp>
#include <kdl/treefksolverpos_recursive.hpp>
#include <kdl/treeiksolverpos_nr_jl.hpp>
#include <kdl/treeiksolvervel_wdls.hpp>

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

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

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

    /** \class se2ez::IKSolverPtr
        \brief A shared pointer wrapper for se2ez::IKSolver. */

    /** \class se2ez::IKSolverConstPtr
        \brief A const shared pointer wrapper for se2ez::IKSolver. */

    /** \brief Common components of IK solvers.
     */
    class IKSolver
    {
    public:
        /** \brief A bitmask specification of what task components should this IK request satisfy.
         */
        enum Request
        {
            NONE = 0,                    ///< No components.
            X_AXIS = 1 << 0,             ///< X component.
            Y_AXIS = 1 << 1,             ///< Y component.
            ROTATION = 1 << 2,           ///< Rotation component.
            POSITION = X_AXIS | Y_AXIS,  ///< X- and Y-axis components.
            ALL = POSITION | ROTATION    ///< All components.
        };

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

        /** \brief Sets the number of random restarts to use when solving a request
         *  \param[in] restarts The number of random restarts to use.
         */
        void setRestarts(unsigned int restarts);

        /** \brief Sets the weights on a specific joint in the robot for IK solving.
         *  \param[in] frame Frame to set weights for.
         *  \param[in] weights Weights to use for frame.
         */
        virtual void setJointWeight(const std::string &frame, const Eigen::VectorXd &weights);

        /** \brief Sets the weights for all joints in the robot for IK solving.
         *  \param[in] weights Weights to use.
         */
        virtual void setJointWeights(const Eigen::VectorXd &weights);

        /** \brief Sets lambda, damping constant in WDLS.
         *  \param[in] lambda Lambda to use.
         */
        virtual void setLambda(double lambda);

    protected:
        /** \brief KDL specific indices for different components of the task matrix.
         */
        enum Index
        {
            POS_X = 0,  ///< X Position
            POS_Y = 1,  ///< Y Position
            POS_Z = 2,  ///< Z Position, Ignored as we are 2D
            ROT_X = 3,  ///< X Rotation, Ignored as we are 2D
            ROT_Y = 4,  ///< Y Rotation, Ignored as we are 2D
            ROT_Z = 5,  ///< Z Rotation
        };

        /** \brief Constructor. Protected so only derived classes can create this.
         */
        IKSolver(const RobotPtr &robot, unsigned int frames);

        /** \brief Sets the task matrix values for a task at index \e task according to a \e request.
         *  \param[in] task Index of task to set.
         *  \param[in] request Request to set.
         */
        void setRequest(unsigned int task, Request request);

        /** \brief Sets the specific \e index component of a \e task to \e active.
         *  \param[in] task Index of task to set.
         *  \param[in] index Index of component to set.
         *  \param[in] active Value to set component to.
         */
        void setTask(unsigned int task, Index index, bool active);

        /** \brief Sets a KDL array to random values between joint limits.
         *  \param[out] array Array to randomly set.
         */
        void setRandom(KDL::JntArray &array) const;

        const unsigned int n_;  ///< Number of joint variables.
        const unsigned int t_;  ///< Number of tasks.
        const RobotPtr robot_;  ///< Reference to robot.

        const KDL::JntArray min_;  ///< Joint minimums.
        const KDL::JntArray max_;  ///< Joint maximums.

        KDL::MatrixXd weights_;  ///< Joint variable weights.
        KDL::MatrixXd tasks_;    ///< Task weights.
        double lambda_{0.01};    ///< Lambda, WDLS constant.

        unsigned int restarts_{10};  ///< Number of random restarts.
    };

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

    /** \class se2ez::TreeIKPtr
        \brief A shared pointer wrapper for se2ez::TreeIK. */

    /** \class se2ez::TreeIKConstPtr
        \brief A const shared pointer wrapper for se2ez::TreeIK. */

    class TreeIK : public IKSolver
    {
    public:
        /** \brief Constructor.
         *  \param[in] robot Robot to construct IK solver for.
         *  \param[in] frames Frames to use as task frames.
         *  \param[in] iterations Maximum number of iterations to use in solver.
         *  \param[in] eps Solution tolerance.
         */
        TreeIK(const RobotPtr &robot, const std::vector<std::string> &frames, unsigned int iterations = 50,
               double eps = 1e-4);

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

        /** \brief Solve an IK request.
         *  \param[in,out] state State to set to result of IK query. Input value is used as first seed.
         *  \param[in] frames Map of frame name to desired values for query.
         *  \param[in] request Map of frame names to requested components. Uses ALL by default.
         *  \return True on success, false on failure.
         */
        bool solve(StatePtr state, const tf::EigenMap<std::string, Eigen::Isometry2d> &frames,
                   const std::map<std::string, Request> &request = {});

        /** \brief Sets the weights on a specific joint in the robot for IK solving.
         *  \param[in] frame Frame to set weights for.
         *  \param[in] weights Weights to use for frame.
         */
        void setJointWeight(const std::string &frame, const Eigen::VectorXd &weights) override;

        /** \brief Sets the weights for all joints in the robot for IK solving.
         *  \param[in] weights Weights to use.
         */
        void setJointWeights(const Eigen::VectorXd &weights) override;

        /** \brief Sets lambda, damping constant in WDLS.
         *  \param[in] lambda Lambda to use.
         */
        void setLambda(double lambda) override;

    protected:
        /** \brief Sets current task matrix and goals according to request.
         *  \param[out] goals Goals for IK to set.
         *  \param[in] frames Desired position of task frames.
         *  \param[in] request Request to satisfy.
         *  \return True if possible, false on error.
         */
        bool setTask(KDL::Frames &goals, const tf::EigenMap<std::string, Eigen::Isometry2d> &frames,
                     const std::map<std::string, Request> &request);

        /** \brief Actually solve an IK query.
         *  \param[in] in Initial seed joint angles.
         *  \param[out] out Output joint angles.
         *  \param[in] goals Goals to satisfy.
         *  \return True on success false on failure.
         */
        virtual bool doIK(KDL::JntArray &in, KDL::JntArray &out, const KDL::Frames &goals);

        const std::map<std::string, unsigned int> frames_;  ///< Mapping of task frame name to index.

        const KDL::Tree tree_;               ///< KDL tree used for solving.
        KDL::TreeFkSolverPos_recursive fk_;  ///< Forward kinematics solver.
        KDL::TreeIkSolverVel_wdls velIK_;    ///< Inverse velocity solver.
        KDL::TreeIkSolverPos_NR_JL posIK_;   ///< Inverse position solver.

        std::mutex mutex_;  ///< Thread mutex
    };

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

    /** \class se2ez::ChainIKPtr
        \brief A shared pointer wrapper for se2ez::ChainIK. */

    /** \class se2ez::ChainIKConstPtr
        \brief A const shared pointer wrapper for se2ez::ChainIK. */

    /** \brief Convenience wrapper around TreeIK for solving chains with a single tip frame.
     */
    class ChainIK : public TreeIK
    {
    public:
        /** \brief Constructor.
         *  \param[in] robot Robot to construct IK solver for.
         *  \param[in] frame Frame to use as task frame.
         *  \param[in] iterations Maximum number of iterations to use in solver.
         *  \param[in] eps Solution tolerance.
         */
        ChainIK(const RobotPtr &robot, const std::string &frame, unsigned int iterations = 50,
                double eps = 1e-4);

        /** \brief Destructor.
         */
        virtual ~ChainIK();

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

        /** \brief Solve an IK request.
         *  \param[in,out] state State to set to result of IK query. Input value is used as first seed.
         *  \param[in] frame Desired value to query.
         *  \param[in] request Requested components. ALL by default.
         *  \return True on success, false on failure.
         */
        bool solve(StatePtr state, const Eigen::Isometry2d &frame, Request request = ALL);

    private:
        const std::string frame_;  ///< Task frame.
    };

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

    /** \class se2ez::CollisionAwareTreeIKPtr
        \brief A shared pointer wrapper for se2ez::CollisionAwareTreeIK. */

    /** \class se2ez::CollisionAwareTreeIKConstPtr
        \brief A const shared pointer wrapper for se2ez::CollisionAwareTreeIK. */

    /** \brief A collision aware version of tree IK.
     */
    class CollisionAwareTreeIK : public TreeIK
    {
    public:
        /** \brief Constructor.
         *  \param[in] robot Robot to construct IK solver for.
         *  \param[in] frames Frames to use as task frames.
         *  \param[in] iterations Maximum number of iterations to use in solver.
         *  \param[in] eps Solution tolerance.
         */
        CollisionAwareTreeIK(const RobotPtr &robot, const std::vector<std::string> &frames,
                             unsigned int iterations = 50, double eps = 1e-4);

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

    protected:
        bool doIK(KDL::JntArray &in, KDL::JntArray &out, const KDL::Frames &goals) override;

    private:
        CollisionManagerPtr cm_;  ///< Collision Manager for IK.
        StatePtr state_;          ///< Work state.
    };

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

    /** \class se2ez::CollisionAwareChainIKPtr
        \brief A shared pointer wrapper for se2ez::CollisionAwareChainIK. */

    /** \class se2ez::CollisionAwareChainIKConstPtr
        \brief A const shared pointer wrapper for se2ez::CollisionAwareChainIK. */

    /** \brief Convenience wrapper around CollisionAwareTreeIK for solving chains with a single tip frame.
     */
    class CollisionAwareChainIK : public CollisionAwareTreeIK
    {
    public:
        /** \brief Constructor.
         *  \param[in] robot Robot to construct IK solver for.
         *  \param[in] frame Frame to use as task frame.
         *  \param[in] iterations Maximum number of iterations to use in solver.
         *  \param[in] eps Solution tolerance.
         */
        CollisionAwareChainIK(const RobotPtr &robot, const std::string &frame, unsigned int iterations = 50,
                              double eps = 1e-4);

        /** \brief Destructor.
         */
        virtual ~CollisionAwareChainIK();

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

        /** \brief Solve an IK request.
         *  \param[in,out] state State to set to result of IK query. Input value is used as first seed.
         *  \param[in] frame Desired value to query.
         *  \param[in] request Requested components. ALL by default.
         *  \return True on success, false on failure.
         */
        bool solve(StatePtr state, const Eigen::Isometry2d &frame, Request request = ALL);

    private:
        const std::string frame_;  ///< Task frame.
    };
}  // namespace se2ez

#endif