RobotIK.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotComponents::ArmarXObjects::RobotIK
 * @author     Joshua Haustein ( joshua dot haustein at gmail dot com )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
 
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
#include <RobotComponents/interface/components/RobotIK.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
 
#include <VirtualRobot/IK/GenericIKSolver.h>
 
#include <mutex>
 
namespace armarx
{
    /**
     * \class RobotIKPropertyDefinition
     * \brief
     */
    class RobotIKPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        RobotIKPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
            //Define optional properties
            defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
            defineOptionalProperty<std::string>("ReachabilitySpacesFolder", "Path to a folder containing reachability spaces");
            defineOptionalProperty<int>("NumIKTrials", 10, "Number of trials to find an ik solution");
            defineOptionalProperty<std::string>("InitialReachabilitySpaces", "", "Reachability spaces to load initially (semi-colon separated)");
        }
    };
 
    /**
     * \defgroup Component-RobotIK RobotIK
     * \ingroup RobotComponents-Components
     * \brief Provides IK solving methods from VirtualRobot (see [Simox](http://simox.sourceforge.net/)).
     *
     * RobotIK provides a set of functions that allow computing IKs for any kinematic chain
     * in a robot. This component requires the following properties:
     *  - RobotFileName: The VirtualRobot robot model to be used.
     *  - RobotStateComponentName: The name of a robot state component.
     *
     * \image html Armar3_IK_small.png "An exemplary IK task and its solution."
     *
     * Furthermore, there are the following optional properties:
     *  - ReachabilitySpacesFolder: A Path to a folder containing reachability spaces.
     *  - NumIKTrials: The number of trials the underlying ik solver tries to find a solution before giving up (default: 10).
     *
     * The robot model is required to solve the IK. Some of the provided functionalities require the current robot state,
     * which is retrieved from the robot state component specified in the properties. These functions will only work properly,
     * if the robot model used by the robot state component is identical to the robot model of this component.
     *
     * The functionalities include:
     *  - computation of an IK solution for a kinematic chain given a goal TCP pose.
     *  - computation of an IK solution for a set of robot joints such that
     *  the projection of the center of mass to the support surface lies within a goal region.
     *  - computation of a joint value gradient to minimize the workspace error for several tasks simultaneously.
     *  - creation and query of reachability spaces for kinematic chains.
     *
     * A reachability space for a kinematic chain allows a fast check whether
     * a TCP pose is reachable. Creating a reachability space, however, is a computational intensive process
     * that should be done offline. You may create a reachability space by calling the respective function
     * or by specifying a path to a folder, ReachabilitySpacesFolder, that contains a set of reachability spaces.
     * If ReachabilitySpacesFolder is specified, each file in the folder is attempted to be loaded as reachability space
     * during initialization.
     *
     * While this implementation allows parallel access to its interface functions,
     * it is not optimized towards maximizing parallelism.
     * If you need to access this component from a large number of threads/processes
     * and you care about performance, you should consider creating multiple instances
     * of this component or using local instances of the IK solver from Simox instead.
     *
     * Furthermore, at the beginning of an operation this component synchronizes its
     * internal robot state with that provided by the robot state component.
     * This can lead to invalid IK solutions if the robot state changes while computation is still in progress.
     * For example, imagine you compute an IK solution just
     * for the forearm and the wrist given a global eef pose. If during the computation the configuration
     * of the elbow changes, the computed ik solution will no longer achieve the desired eef pose.
     *
     * Also see [Simox-Tutorial] (http://sourceforge.net/p/simox/wiki/VirtualRobot/) for more information
     * of how to use Simox for IK solving.
     */
 
    /**
     * @ingroup Component-RobotIK
     * @brief Refer to \ref Component-RobotIK
     */
    class ARMARXCOMPONENT_IMPORT_EXPORT RobotIK :
        virtual public Component,
        virtual public RobotIKInterface
    {
    public:
 
        /**
         * \return the robot xml filename as specified in the configuration
         */
        virtual std::string getRobotFilename(const Ice::Current&) const;
 
        /**
         * Create an instance of RobotIKPropertyDefinitions.
         */
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        std::string getDefaultName() const override
        {
            return "RobotIK";
        }
 
        /**
         * @brief Computes a single IK solution for the given robot node set and desired TCP pose.
         * @details The TCP pose can be defined in any frame and is converted internally to a global pose
         * according to the current robot state. The CartesianSelection
         * parameter defines which part of the target pose should be reached.
         *
         * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
         * within the robot model or has been defined via \ref defineRobotNodeSet.
         * @param tcpPose The framed target pose for the TCP.
         * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
         *           the orientation only or all.
         * @return A map that maps each joint name to its value in the found IK solution.
         */
        NameValueMap computeIKFramedPose(const std::string& robotNodeSetName,
                                         const FramedPoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Computes a single IK solution for the given robot node set and desired global TCP pose.
         * @details The TCP pose is assumed to be in the global frame. The CartesianSelection
         * parameter defines which part of the target pose should be reached.
         *
         * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
         * within the robot model or has been defined via \ref defineRobotNodeSet.
         * @param tcpPose The global target pose for the TCP.
         * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
         *           the orientation only or all.
         * @return A map that maps each joint name to its value in the found IK solution.
         */
        NameValueMap computeIKGlobalPose(const std::string& robotNodeSetName,
                                         const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Computes a single IK solution, error and reachability for the given robot node set and desired global TCP pose.
         * @details The TCP pose is assumed to be in the global frame. The CartesianSelection
         * parameter defines which part of the target pose should be reached.
         *
         * @param robotNodeSetName The name of a robot node set (robot node set) that is either stored
         * within the robot model or has been defined via \ref defineRobotNodeSet.
         * @param tcpPose The global target pose for the TCP.
         * @param cs Specifies which part of the pose needs to be reached by the IK, e.g. the position only,
         *           the orientation only or all.
         * @return A map that maps each joint name to its value in the found IK solution, the reachability and computational error.
         */
        ExtendedIKResult computeExtendedIKGlobalPose(const std::string& robotNodeSetName,
                const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Computes an IK solution for the given robot joints such that the center of mass lies above the
         * given point.
         * @details
         *
         * @param robotNodeSetJoints Name of the robot node set that contains the joints you wish to compute the IK for.
         * @param comIK A center of mass description. Note that the priority field is relevant for this function as there
         * is only a single CoM of descriptor.
         *
         * @return The ik-solution. Returns an empty vector if there is no solution.
         */
        NameValueMap computeCoMIK(const std::string& robotNodeSetJoints, const CoMIKDescriptor& desc, const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Computes a configuration gradient in order to solve several tasks/constraints simultaneously.
         * @details This function allows you to use the HierarchicalIK solver provided by Simox.
         * It computes a configuration gradient for the given robot node set that minimizes the workspace errors
         * for multiple tasks simultaneously. You can specify IK tasks, a center of mass task and a joint limit avoidance task.
         * For details for the different type of tasks, see the parameter description. You must define a priority for each task.
         * The task with maximal priority is the first task to be solved. Each subsequent task is then solved
         * within the null space of the previous task. <b> Note that this function returns a gradient and NOT
         * an absolute IK solution. The gradient is computed on the current configuration of the robot. <\b>.
         * The gradient is computed from the current robot configuration.
         *
         * See @url http://simox.sourceforge.net/documentation/class_virtual_robot_1_1_hierarchical_i_k.html
         * and @url http://sourceforge.net/p/simox/wiki/VirtualRobot/#hierarchical-ik-solving for more information.
         *
         * @param robotNodeSet The robot node set (e.g. kinematic tree) you wish to compute the gradient for.
         * @param diffIKs A list of IK tasks. Each IK task specifies a TCP and a desired pose for this TCP.
         * @param comIK A center of mass tasks. Defines where the center should be and its priority. Is only used if the
                        CoM-task is enabled.
         * @param stepSize
         * @param avoidJointLimits Set whether or not to avoid joint limits.
         * @param enableCenterOfMass Set whether or not to adjust the center of mass.
         * @return A configuration gradient...
         */
        NameValueMap computeHierarchicalDeltaIK(const std::string& robotNodeSet,
                                                const IKTasks& iktasks, const CoMIKDescriptor& comIK,
                                                float stepSize, bool avoidJointLimits, bool enableCenterOfMass, const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Creates a new reachability space for the given robot node set.
         * @details If there is no reachability space for the given robot node set yet, a new one is created. This may take
         *          some time. The function returns true iff a reachability space for the given robot node set exists after
         *          execution of this function.
         *
         * @param chainName The name of a defined robot node set. This can be either a robot node set that is defined in the
         *                  robot model or a robot node set that has been manually defined calling \ref defineRobotNodeSet.
         * @param coordinateSystem The name of the robot node in whose coordinate system the reachability space shall be defined
         *                  in. If you wish to choose the global coordinate system, pass an empty string.
         *                  Note, however, that any reachability space defined in the
         *                  global coordinate system gets invalidated once the robot moves.
         * @param stepTranslation The extend of a voxel dimension in translational dimensions (x,y,z) [mm]
         * @param stepRotation The extend of a voxel dimension in rotational dimensions (roll,pitch,yaw) [rad]
         * @param minBounds The minimum workspace poses (x,y,z,ro,pi,ya) given in the base node's coordinate system [mm and rad]
         * @param maxBounds The maximum workspace poses (x,y,z,ro,pi,ya) given in base node's coordinate system [mm and rad]
         * @param numSamples The number of tcp samples to take to create the reachability space (e.g 1000000)
         * @return True iff the a reachability space for the given robot node set is available after execution of this function.
         *         False in case of a failure, e.g. there is no chain with the given name.
         */
        bool createReachabilitySpace(const std::string& chainName, const std::string& coordinateSystem, float stepTranslation, float stepRotation,
                                     const WorkspaceBounds& minBounds, const WorkspaceBounds& maxBounds, int numSamples, const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Defines a new robot node set.
         * @details Define a new robot node set with the given name that consists out of the given list of nodes with given
         *          TCP and root frame. Iff the chain is successfully added or already exists, <it>true</it> is returned.
         * @param name The name of the robot node set.
         * @param nodes The list of robot nodes that make up the robot node set.
         * @param tcpName The name of the TCP node.
         * @param rootNode The name of the kinematic root.
         *
         * @return True, iff chain was added or already exists. False, iff a different chain with similar name already exists.
         */
        bool defineRobotNodeSet(const std::string& name, const NodeNameList& nodes,
                                const std::string& tcpName, const std::string& rootNode, const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
        * @brief Returns whether this component has a reachability space for the given robot node set.
        * @details True if there is a reachability space available, else false.
        *
        * @param chainName Name of the robot node set.
        * @return True if there is a reachability space available, else false.
        */
        bool hasReachabilitySpace(const std::string& chainName, const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @brief Returns whether a given framed pose is currently reachable by the TCP of the given robot node set.
         * @details To determine whether a pose is reachable a reachability space for the given robot node set is
         *          required. If no such space exists, the function returns <it>false<\it>.
         *          Call \ref createReachabilitySpace first to ensure there is such a space.
         *
         * @param chainName A name of a robot node set either defined in the robot model or previously defined via
         *                  \ref defineRobotNodeSet.
         * @param framedPose A framed pose to check for reachability. The pose is transformed into a global pose using the
         *                   current robot state.
         *
         * @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
         *         there is no reachability space for the given chain available.
         */
        bool isFramedPoseReachable(const std::string& chainName, const FramedPoseBasePtr& tcpPose, const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @brief Returns whether a given global pose is currently reachable by the TCP of the given robot node set.
         * @details To determine whether a pose is reachable a reachability space for the given robot node set is
         *          required. If no such space exists, the function returns <it>false<\it>.
         *          Call \ref createReachabilitySpace first to ensure there is such a space.
         *
         * @param chainName A name of a robot node set either defined in the robot model or previously defined via
         *                  \ref defineRobotNodeSet.
         * @param pose A global pose to check for reachability.
         *
         * @return True, if the pose is reachable by the TCP of the given chain. False, if it is not reachable or
         *         there is no reachability space for the given chain available.
         */
        bool isPoseReachable(const std::string& chainName, const PoseBasePtr& tcpPose, const Ice::Current& = Ice::emptyCurrent) const override;
 
        /**
         * @brief Loads the reachability space from the given file.
         * @details If loading the reachability space succeeds, the reachability space is ready to be used after this function
         *          terminates. A reachability space can only be loaded if there is no reachability space for the respective
         *          robot node set yet.
         *
         * @param filename Binary file containing a reachability space. Ensure that the path is valid and accessible from
         *                 where this component is running.
         * @return True iff loading the reachability space was successful.
         */
        bool loadReachabilitySpace(const std::string& filename, const Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief Saves a previously created reachability space of the given robot node set.
         * @details A reachability space for a robot node set can only be saved, if actually is one.
         *          You can check whether a reachability space is available by calling \ref hasReachabilitySpace(robotNodeSet).
         *
         * @param robotNodeSet The robot node set for which you wish to save the reachability space.
         * @param filename The filename if the file(must be an accessible path for this component) you wish to save the space in.
         * @return True iff saving was successful.
         */
        bool saveReachabilitySpace(const std::string& robotNodeSet, const std::string& filename, const Ice::Current& = Ice::emptyCurrent) const override;
 
 
    protected:
        /**
         * Load and create a VirtualRobot::Robot instance from the RobotFileName
         * property.
         */
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
 
    private:
        bool solveIK(const Eigen::Matrix4f& tcpPose, armarx::CartesianSelection cs, VirtualRobot::RobotNodeSetPtr nodeSet);
        void computeIK(const std::string& robotNodeSetName, const Eigen::Matrix4f& tcpPose,
                       armarx::CartesianSelection cs, NameValueMap& iksolution);
 
        void computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
                       armarx::CartesianSelection cs, NameValueMap& iksolution);
 
        void computeIK(const std::string& robotNodeSetName, const Eigen::Matrix4f& tcpPose,
                       armarx::CartesianSelection cs, ExtendedIKResult& iksolution);
 
        void computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
                       armarx::CartesianSelection cs, ExtendedIKResult& iksolution);
 
        Eigen::Matrix4f toGlobalPose(const FramedPoseBasePtr& tcpPose) const;
        Eigen::Matrix4f toReachabilityMapFrame(const FramedPoseBasePtr& tcpPose, const std::string& chainName) const;
 
        bool isReachable(const std::string& setName, const Eigen::Matrix4f& tcpPose) const;
 
        VirtualRobot::IKSolver::CartesianSelection convertCartesianSelection(armarx::CartesianSelection cs) const;
 
        VirtualRobot::JacobiProvider::InverseJacobiMethod convertInverseJacobiMethod(armarx::InverseJacobiMethod aenum) const;
 
        void synchRobot() const;
 
        // Lock this mutex if you plan to modify the robot model
        // Exception: When adding node sets we don't need to.
        // Invariant: We never delete node sets nor nodes!
        mutable std::recursive_mutex _modifyRobotModelMutex;
        // Lock this mutex if you are adding a node set, to ensure we do not add
        // a similar node set at the same time.
        mutable std::recursive_mutex _editRobotNodeSetsMutex;
        // Lock this mutex if you are reading or editing the reachability cache.
        mutable std::recursive_mutex _accessReachabilityCacheMutex;
        // Needs to be locked when computing IKs! Internally thread safe though.
        VirtualRobot::RobotPtr _robotModel;
        std::string _robotFile;
        RobotStateComponentInterfacePrx _robotStateComponentPrx;
        SharedRobotInterfacePrx _synchronizedRobot;
        DebugDrawerInterfacePrx debugDrawer;
 
        // Reachability cache: not thread safe, always lock!
        using ReachabilityCacheType = std::map<std::string, VirtualRobot::WorkspaceRepresentationPtr>;
        ReachabilityCacheType _reachabilities;
        int _numIKTrials;
    };
 
}