RobotIK.cpp

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 ), Nikolaus Vahrenkamp
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RobotIK.h"
 
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Workspace/Reachability.h>
#include <VirtualRobot/Workspace/Manipulability.h>
#include <VirtualRobot/IK/CoMIK.h>
#include <VirtualRobot/IK/JointLimitAvoidanceJacobi.h>
#include <VirtualRobot/IK/HierarchicalIK.h>
#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
 
#include <filesystem>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <VirtualRobot/IK/constraints/PoseConstraint.h>
#include <VirtualRobot/IK/constraints/JointLimitAvoidanceConstraint.h>
#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
#include <VirtualRobot/IK/constraints/PositionConstraint.h>
 
#include <algorithm>
#include <set>
 
using namespace VirtualRobot;
using namespace Eigen;
using namespace Ice;
 
 
 
namespace armarx
{
 
    void RobotIK::onInitComponent()
    {
        _robotFile = getProperty<std::string>("RobotFileName").getValue();
 
        if (!ArmarXDataPath::getAbsolutePath(_robotFile, _robotFile))
        {
            throw UserException("Could not find robot file " + _robotFile);
        }
 
        this->_robotModel = VirtualRobot::RobotIO::loadRobot(_robotFile, VirtualRobot::RobotIO::eStructure);
 
        if (this->_robotModel)
        {
            ARMARX_VERBOSE << "Loaded robot from file " << _robotFile;
        }
        else
        {
            ARMARX_VERBOSE << "Failed loading robot from file " << _robotFile;
        }
 
        // Get number of ik trials
        _numIKTrials = getProperty<int>("NumIKTrials").getValue();
 
        // Load initial reachability maps (if configured)
        std::string spacesStr = getProperty<std::string>("InitialReachabilitySpaces").getValue();
        std::vector<std::string> spaces = armarx::Split(spacesStr, ";");
 
        if (spacesStr != "")
        {
            std::string spacesFolder = getProperty<std::string>("ReachabilitySpacesFolder").getValue();
 
            for (auto& space : spaces)
            {
                ARMARX_INFO << "Initially loading reachability space '" << (spacesFolder + "/" + space) << "'";
 
                std::string absolutePath;
 
                if (!ArmarXDataPath::getAbsolutePath(spacesFolder + "/" + space, absolutePath))
                {
                    ARMARX_ERROR << "Could not load reachability map '" << (spacesFolder + "/" + space) << "'";
                    continue;
                }
 
                loadReachabilitySpace(absolutePath);
            }
        }
 
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        offeringTopic("DebugDrawerUpdates");
    }
 
 
    void RobotIK::onConnectComponent()
    {
        _robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
 
        std::string robFile_remote = _robotStateComponentPrx->getRobotFilename();
        ArmarXDataPath::getAbsolutePath(robFile_remote, robFile_remote);
 
 
        if (robFile_remote.compare(_robotFile) != 0)
        {
            ARMARX_WARNING << "The robot state component uses the robot model " << robFile_remote
                           << " This component, however, uses " << _robotFile << " Both models must be identical!";
        }
 
        _synchronizedRobot = _robotStateComponentPrx->getSynchronizedRobot();
        debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
 
    }
 
    void RobotIK::onDisconnectComponent()
    {
    }
 
    PropertyDefinitionsPtr RobotIK::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new RobotIKPropertyDefinitions(
                                          getConfigIdentifier()));
    }
 
    NameValueMap RobotIK::computeIKFramedPose(const std::string& robotNodeSetName,
            const FramedPoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current&)
    {
        NameValueMap ikSolution;
        computeIK(robotNodeSetName, toGlobalPose(tcpPose), cs, ikSolution);
        return ikSolution;
    }
 
    NameValueMap RobotIK::computeIKGlobalPose(const std::string& robotNodeSetName,
            const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current&)
    {
        NameValueMap ikSolution;
        Pose globalTcpPose(tcpPose->position, tcpPose->orientation);
        computeIK(robotNodeSetName, globalTcpPose.toEigen(), cs, ikSolution);
        return ikSolution;
    }
 
    ExtendedIKResult RobotIK::computeExtendedIKGlobalPose(const std::string& robotNodeSetName,
            const PoseBasePtr& tcpPose, armarx::CartesianSelection cs, const Ice::Current&)
    {
        ExtendedIKResult ikSolution;
        Pose globalTcpPose(tcpPose->position, tcpPose->orientation);
        computeIK(robotNodeSetName, globalTcpPose.toEigen(), cs, ikSolution);
        return ikSolution;
    }
 
    NameValueMap RobotIK::computeCoMIK(const std::string& robotNodeSetJoints, const CoMIKDescriptor& desc, const Ice::Current&)
    {
        // Make sure we have valid input parameters
        if (!_robotModel->hasRobotNodeSet(robotNodeSetJoints))
        {
            return NameValueMap();
        }
 
        if (!_robotModel->hasRobotNodeSet(desc.robotNodeSetBodies))
        {
            return NameValueMap();
        }
 
        RobotNodePtr coordSystem = RobotNodePtr();
 
        if (desc.coordSysName.size() > 0 && _robotModel->hasRobotNode(desc.coordSysName))
        {
            coordSystem = _robotModel->getRobotNode(desc.coordSysName);
        }
 
        // Create and initialize ik solver
        RobotNodeSetPtr joints = _robotModel->getRobotNodeSet(robotNodeSetJoints);
        CoMIK comIkSolver(joints, _robotModel->getRobotNodeSet(desc.robotNodeSetBodies), coordSystem);
        Eigen::VectorXf goal(2);
        goal(0) = desc.gx;
        goal(1) = desc.gy;
        comIkSolver.setGoal(goal, desc.tolerance);
 
        // Solve
        std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
        bool success = comIkSolver.solveIK();
        NameValueMap result;
 
        if (success)
        {
            for (auto& joint : joints->getAllRobotNodes())
            {
                NameValueMap::value_type jointPair(joint->getName(), joint->getJointValue());
                result.insert(jointPair);
            }
        }
 
        return result;
    }
 
    NameValueMap RobotIK::computeHierarchicalDeltaIK(const std::string& robotNodeSetName,
            const IKTasks& iktasks, const CoMIKDescriptor& comIK, float stepSize,
            bool avoidJointLimits, bool enableCenterOfMass, const Ice::Current&)
    {
        using PriorityJacobiProviderPair = std::pair<int, JacobiProviderPtr>;
        auto lowerPriorityCompare = [](const PriorityJacobiProviderPair & a, const PriorityJacobiProviderPair & b)
        {
            return a.first < b.first;
        };
        std::multiset<PriorityJacobiProviderPair, decltype(lowerPriorityCompare)> jacobiProviders(lowerPriorityCompare);
 
        if (!_robotModel->hasRobotNodeSet(robotNodeSetName))
        {
            throw UserException("Unknown robot node set " + robotNodeSetName);
        }
 
        synchRobot();
 
        RobotNodeSetPtr rns = _robotModel->getRobotNodeSet(robotNodeSetName);
        // First add all ik tasks
        for (DifferentialIKDescriptor const& ikTask : iktasks)
        {
            if (!_robotModel->hasRobotNode(ikTask.tcpName))
            {
                throw UserException("Unknown TCP: " + ikTask.tcpName);
            }
 
            RobotNodePtr coordSystem = RobotNodePtr();
 
            if (ikTask.coordSysName.size() > 0 && _robotModel->hasRobotNode(ikTask.coordSysName))
            {
                coordSystem = _robotModel->getRobotNode(ikTask.coordSysName);
            }
 
            DifferentialIKPtr diffIK(new DifferentialIK(rns, coordSystem, convertInverseJacobiMethod(ikTask.ijm)));
            Pose globalTcpPose(ikTask.tcpGoal->position, ikTask.tcpGoal->orientation);
            RobotNodePtr tcp = _robotModel->getRobotNode(ikTask.tcpName);
            diffIK->setGoal(globalTcpPose.toEigen(), tcp, convertCartesianSelection(ikTask.csel));
            JacobiProviderPtr jacoProv = diffIK;
            jacobiProviders.insert(PriorityJacobiProviderPair(ikTask.priority, jacoProv));
        }
 
        // Now add the center of mass task
        if (enableCenterOfMass)
        {
            if (!_robotModel->hasRobotNodeSet(comIK.robotNodeSetBodies))
            {
                throw UserException("Unknown robot node set for bodies: " + comIK.robotNodeSetBodies);
            }
 
            RobotNodePtr coordSystem = RobotNodePtr();
 
            if (comIK.coordSysName.size() > 0 && _robotModel->hasRobotNode(comIK.coordSysName))
            {
                coordSystem = _robotModel->getRobotNode(comIK.coordSysName);
            }
 
            CoMIKPtr comIkSolver(new CoMIK(rns, _robotModel->getRobotNodeSet(comIK.robotNodeSetBodies)));
            Eigen::VectorXf goal(2);
            goal(0) = comIK.gx;
            goal(1) = comIK.gy;
            comIkSolver->setGoal(goal, comIK.tolerance);
            JacobiProviderPtr jacoProv = comIkSolver;
            jacobiProviders.insert(PriorityJacobiProviderPair(comIK.priority, jacoProv));
        }
 
        std::vector<JacobiProviderPtr> jacobies;
        for (PriorityJacobiProviderPair const& pair : jacobiProviders)
        {
            jacobies.push_back(pair.second);
        }
 
        if (avoidJointLimits)
        {
            JointLimitAvoidanceJacobiPtr avoidanceJacobi(new JointLimitAvoidanceJacobi(rns));
            jacobies.push_back(avoidanceJacobi);
        }
 
        std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
        HierarchicalIK hik(rns);
        Eigen::VectorXf delta = hik.computeStep(jacobies, stepSize);
        NameValueMap result;
 
        int index = 0;
        for (RobotNodePtr const& node : rns->getAllRobotNodes())
        {
            NameValueMap::value_type jointPair(node->getName(), delta(index));
            result.insert(jointPair);
            ++index;
        }
        return result;
    }
 
    bool RobotIK::createReachabilitySpace(const std::string& chainName, const std::string& coordinateSystem, float stepTranslation,
                                          float stepRotation, const WorkspaceBounds& minBounds, const WorkspaceBounds& maxBounds, int numSamples, const Ice::Current&)
    {
        std::lock_guard<std::recursive_mutex> cacheLock(_accessReachabilityCacheMutex);
 
        if (_reachabilities.count(chainName) == 0)
        {
            if (!_robotModel->hasRobotNodeSet(chainName))
            {
                return false;
            }
 
            //VirtualRobot::WorkspaceRepresentationPtr reachability(new Reachability(_robotModel));
            VirtualRobot::WorkspaceRepresentationPtr reachability(new Manipulability(_robotModel));
            float minBoundsArray[] = {minBounds.x, minBounds.y, minBounds.z, minBounds.ro, minBounds.pi, minBounds.ya};
            float maxBoundsArray[] = {maxBounds.x, maxBounds.y, maxBounds.z, maxBounds.ro, maxBounds.pi, maxBounds.ya};
 
            std::lock_guard<std::recursive_mutex> robotLock(_modifyRobotModelMutex);
 
            // TODO add collision checks
            if (coordinateSystem.size() > 0)
            {
                if (!_robotModel->hasRobotNode(coordinateSystem))
                {
                    ARMARX_ERROR << "Unknown coordinate system " << coordinateSystem;
                    return false;
                }
 
                reachability->initialize(_robotModel->getRobotNodeSet(chainName), stepTranslation, stepRotation, minBoundsArray, maxBoundsArray,
                                         VirtualRobot::SceneObjectSetPtr(), VirtualRobot::SceneObjectSetPtr(), _robotModel->getRobotNode(coordinateSystem));
            }
            else
            {
                reachability->initialize(_robotModel->getRobotNodeSet(chainName), stepTranslation, stepRotation, minBoundsArray, maxBoundsArray);
                ARMARX_WARNING << "Using global coordinate system to create reachability space.";
            }
 
            reachability->addRandomTCPPoses(numSamples);
            _reachabilities.insert(ReachabilityCacheType::value_type(chainName, reachability));
        }
 
        return true;
    }
 
    bool RobotIK::defineRobotNodeSet(const std::string& name, const NodeNameList& nodes,
                                     const std::string& tcpName, const std::string& rootNodeName, const Ice::Current&)
    {
        auto stringsCompareEqual = [](const std::string & a, const std::string & b)
        {
            return a.compare(b) == 0;
        };
        auto stringsCompareSmaller = [](const std::string & a, const std::string & b)
        {
            return a.compare(b) <= 0;
        };
        // First check if there is already a set with the given name
        // We need to lock here, to make sure we are not adding similar named sets at the same time.
        std::lock_guard<std::recursive_mutex> lock(_editRobotNodeSetsMutex);
 
        if (_robotModel->hasRobotNodeSet(name))
        {
            // If so, check if the set is identical to the one we plan to add
            bool setsIdentical = true;
            RobotNodeSetPtr rns = _robotModel->getRobotNodeSet(name);
            // Check TCP node
            RobotNodePtr tcpNode = rns->getTCP();
            setsIdentical &= stringsCompareEqual(tcpNode->getName(), tcpName);
            // Check Root node
            RobotNodePtr rootNode = rns->getKinematicRoot();
            setsIdentical &= stringsCompareEqual(rootNode->getName(), rootNodeName);
            // Check remaining nodes
            std::vector<std::string> nodeNames;
            for (RobotNodePtr const& robotNode : rns->getAllRobotNodes())
            {
                nodeNames.push_back(robotNode->getName());
            }
            // TODO check if sorting actually makes sense here
            std::sort(nodeNames.begin(), nodeNames.end(), stringsCompareSmaller);
            std::vector<std::string> inputNodeNames(nodes);
            std::sort(inputNodeNames.begin(), inputNodeNames.end(), stringsCompareSmaller);
            std::pair< std::vector<std::string>::iterator, std::vector<std::string>::iterator > mismatch;
            mismatch = std::mismatch(nodeNames.begin(), nodeNames.end(), inputNodeNames.begin(), stringsCompareEqual);
            setsIdentical &= mismatch.first == nodeNames.end() && mismatch.second == inputNodeNames.end();
 
            return setsIdentical;
        }
 
        // Else we can add the new robot node set
        RobotNodeSetPtr rns = RobotNodeSet::createRobotNodeSet(_robotModel, name, nodes, rootNodeName, tcpName, true);
        return _robotModel->hasRobotNodeSet(name);
    }
 
    std::string RobotIK::getRobotFilename(const Ice::Current&) const
    {
        return _robotFile;
    }
 
    bool RobotIK::hasReachabilitySpace(const std::string& chainName, const Ice::Current&) const
    {
        std::lock_guard<std::recursive_mutex> lock(_accessReachabilityCacheMutex);
        return _reachabilities.count(chainName) > 0;
    }
 
    bool RobotIK::isFramedPoseReachable(const std::string& chainName, const FramedPoseBasePtr& tcpPose, const Ice::Current&) const
    {
        return isReachable(chainName, toReachabilityMapFrame(tcpPose, chainName));
    }
 
    bool RobotIK::isPoseReachable(const std::string& chainName, const PoseBasePtr& tcpPose, const Ice::Current&) const
    {
        Pose globalTcpPose(tcpPose->position, tcpPose->orientation);
        return isReachable(chainName, globalTcpPose.toEigen());
    }
 
    bool RobotIK::loadReachabilitySpace(const std::string& filename, const Ice::Current&)
    {
        VirtualRobot::WorkspaceRepresentationPtr newSpace;
        bool success = false;
 
        // 1st try to load as manipulability file
        try
        {
            newSpace.reset(new Manipulability(_robotModel));
            newSpace->load(filename);
            success = true;
 
            ARMARX_INFO << "Map '" << filename << "' loaded as Manipulability map";
        }
        catch (...)
        {
        }
 
        // 2nd try to load as reachability file
        if (!success)
        {
            try
            {
                newSpace.reset(new Reachability(_robotModel));
                newSpace->load(filename);
                success = true;
 
                ARMARX_INFO << "Map '" << filename << "' loaded as Reachability map";
            }
            catch (...)
            {
            }
        }
 
        if (!success)
        {
            ARMARX_ERROR << "Failed to load map '" << filename << "'";
            return false;
        }
 
        try
        {
            std::lock_guard<std::recursive_mutex> lock(_accessReachabilityCacheMutex);
            std::string chainName = newSpace->getNodeSet()->getName();
 
            if (_reachabilities.count(chainName) == 0)
            {
                _reachabilities.insert(ReachabilityCacheType::value_type(chainName, newSpace));
            }
            else
            {
                ARMARX_WARNING << "Reachability map for kinematic chain '" << chainName << "' already loaded";
                return false;
            }
        }
        catch (Exception&)
        {
            throw;
        }
 
        return true;
    }
 
    bool RobotIK::saveReachabilitySpace(const std::string& robotNodeSet, const std::string& filename, const Ice::Current&) const
    {
        std::lock_guard<std::recursive_mutex> lock(_accessReachabilityCacheMutex);
        bool success = false;
 
        if (_reachabilities.count(robotNodeSet) > 0)
        {
            ReachabilityCacheType::const_iterator it = _reachabilities.find(robotNodeSet);
 
            try
            {
                std::filesystem::path savePath(filename);
                std::filesystem::create_directories(savePath.parent_path());
                it->second->save(filename);
                success = true;
            }
            catch (Exception&)
            {
                // no need to unlock, this is done automatically once we leave this context
                throw;
            }
        }
 
        return success;
    }
 
    void RobotIK::computeIK(const std::string& robotNodeSetName, const Eigen::Matrix4f& tcpPose,
                            armarx::CartesianSelection cs, NameValueMap& ikSolution)
    {
        if (!_robotModel->hasRobotNodeSet(robotNodeSetName))
        {
            throw UserException("The robot model does not contain the robot node set " + robotNodeSetName);
        }
 
        RobotNodeSetPtr rns = _robotModel->getRobotNodeSet(robotNodeSetName);
        computeIK(rns, tcpPose, cs, ikSolution);
    }
 
    void RobotIK::computeIK(const std::string& robotNodeSetName, const Eigen::Matrix4f& tcpPose,
                            armarx::CartesianSelection cs, ExtendedIKResult& ikSolution)
    {
        if (!_robotModel->hasRobotNodeSet(robotNodeSetName))
        {
            throw UserException("The robot model does not contain the robot node set " + robotNodeSetName);
        }
 
        RobotNodeSetPtr rns = _robotModel->getRobotNodeSet(robotNodeSetName);
        computeIK(rns, tcpPose, cs, ikSolution);
    }
 
    void RobotIK::computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
                            armarx::CartesianSelection cs, NameValueMap& ikSolution)
    {
 
        ikSolution.clear();
        // For the rest of this function we need to lock access to the robot,
        // because we need to make sure we read the correct result from the robot node set.
        std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
        // Synch the internal robot state with that of the robot state component
        synchRobot();
 
        bool success = solveIK(tcpPose, cs, nodeSet);
 
 
        //        GenericIKSolver ikSolver(nodeSet, JacobiProvider::eSVDDamped);
        //        bool success = ikSolver.solve(tcpPose, convertCartesianSelection(cs), _numIKTrials);
 
        // Read solution from node set
        if (success)
        {
            const std::vector<RobotNodePtr> robotNodes = nodeSet->getAllRobotNodes();
            for (RobotNodePtr const& rnode : robotNodes)
            {
                NameValueMap::value_type jointPair(rnode->getName(), rnode->getJointValue());
                ikSolution.insert(jointPair);
            }
        }
 
        // Lock is automatically released
    }
 
    bool RobotIK::solveIK(const Eigen::Matrix4f& tcpPose, armarx::CartesianSelection cs, VirtualRobot::RobotNodeSetPtr nodeSet)
    {
        //        VirtualRobot::ConstraintPtr poseConstraint(new VirtualRobot::PoseConstraint(_robotModel, nodeSet, nodeSet->getTCP(),
        //                tcpPose, convertCartesianSelection(cs)));
 
        std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
        VirtualRobot::ConstraintPtr posConstraint(new VirtualRobot::PositionConstraint(_robotModel, nodeSet, nodeSet->getTCP(),
                tcpPose.block<3, 1>(0, 3), convertCartesianSelection(cs)));
        posConstraint->setOptimizationFunctionFactor(1);
 
        VirtualRobot::ConstraintPtr oriConstraint(new VirtualRobot::OrientationConstraint(_robotModel, nodeSet, nodeSet->getTCP(),
                tcpPose.block<3, 3>(0, 0), convertCartesianSelection(cs), VirtualRobot::MathTools::deg2rad(2)));
        oriConstraint->setOptimizationFunctionFactor(1000);
 
        Eigen::VectorXf jointConfig;
        nodeSet->getJointValues(jointConfig);
        VirtualRobot::ConstraintPtr referenceConfigConstraint(new VirtualRobot::ReferenceConfigurationConstraint(_robotModel, nodeSet, jointConfig));
        referenceConfigConstraint->setOptimizationFunctionFactor(0.1);
 
        VirtualRobot::ConstraintPtr jointLimitAvoidanceConstraint(new VirtualRobot::JointLimitAvoidanceConstraint(_robotModel, nodeSet));
        jointLimitAvoidanceConstraint->setOptimizationFunctionFactor(0.1);
 
        // Instantiate solver and generate IK solution
        VirtualRobot::ConstrainedOptimizationIK ikSolver(_robotModel, nodeSet, 0.5, 0.03);
        ikSolver.addConstraint(posConstraint);
        ikSolver.addConstraint(oriConstraint);
        ikSolver.addConstraint(jointLimitAvoidanceConstraint);
        ikSolver.addConstraint(referenceConfigConstraint);
 
        ikSolver.initialize();
        bool success = ikSolver.solve();
        if (!success) // try again with no soft-constraints, which distract the optimizer
        {
            VirtualRobot::ConstrainedOptimizationIK ikSolver(_robotModel, nodeSet, 0.5, 0.03);
            ikSolver.addConstraint(posConstraint);
            ikSolver.addConstraint(oriConstraint);
 
            ikSolver.initialize();
            success = ikSolver.solve();
        }
        return success;
    }
 
    void RobotIK::computeIK(VirtualRobot::RobotNodeSetPtr nodeSet, const Eigen::Matrix4f& tcpPose,
                            armarx::CartesianSelection cs, ExtendedIKResult& ikSolution)
    {
        ikSolution.jointAngles.clear();
        // For the rest of this function we need to lock access to the robot,
        // because we need to make sure we read the correct result from the robot node set.
        std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
        // Synch the internal robot state with that of the robot state component
        synchRobot();
 
        bool success = solveIK(tcpPose, cs, nodeSet);
 
        // Read solution from node set
        const std::vector<RobotNodePtr> robotNodes = nodeSet->getAllRobotNodes();
        for (RobotNodePtr const& rnode : robotNodes)
        {
            NameValueMap::value_type jointPair(rnode->getName(), rnode->getJointValue());
            ikSolution.jointAngles.insert(jointPair);
        }
 
        //Calculate error
        ikSolution.errorPosition = (nodeSet->getTCP()->getGlobalPose().block<3, 1>(0, 3) - tcpPose.block<3, 1>(0, 3)).norm();
        ikSolution.errorOrientation = Eigen::AngleAxisf(nodeSet->getTCP()->getGlobalPose().block<3, 3>(0, 0) * tcpPose.block<3, 3>(0, 0).inverse()).angle();
 
        ikSolution.isReachable = success;
        // Lock is automatically released
    }
 
 
    Eigen::Matrix4f RobotIK::toGlobalPose(const FramedPoseBasePtr& tcpPose) const
    {
        FramedPose framedTcpPose(tcpPose->position, tcpPose->orientation, tcpPose->frame, tcpPose->agent);
        FramedPosePtr globalTcpPose = framedTcpPose.toGlobal(_synchronizedRobot);
        return globalTcpPose->toEigen();
    }
 
    Eigen::Matrix4f RobotIK::toReachabilityMapFrame(const FramedPoseBasePtr& tcpPose, const std::string& chainName) const
    {
        FramedPosePtr p = FramedPosePtr::dynamicCast(tcpPose);
 
        PosePtr p_global(new Pose(toGlobalPose(tcpPose)));
        debugDrawer->setPoseDebugLayerVisu("Grasp Pose", p_global);
 
        std::lock_guard<std::recursive_mutex> lock(_accessReachabilityCacheMutex);
 
        if (_reachabilities.count(chainName))
        {
            ReachabilityCacheType::const_iterator it = _reachabilities.find(chainName);
 
            p->changeFrame(_synchronizedRobot, it->second->getBaseNode()->getName());
        }
        else
        {
            ARMARX_WARNING << "Could not convert TCP pose to reachability map frame: Map not found.";
        }
 
        return p->toEigen();
    }
 
    bool RobotIK::isReachable(const std::string& setName, const Eigen::Matrix4f& tcpPose) const
    {
        ARMARX_INFO << "Checking reachability for kinematic chain '" << setName << "': " << tcpPose;
 
        std::lock_guard<std::recursive_mutex> lock(_accessReachabilityCacheMutex);
 
        if (_reachabilities.count(setName))
        {
            ReachabilityCacheType::const_iterator it = _reachabilities.find(setName);
            return it->second->isCovered(tcpPose);
        }
        else
        {
            ARMARX_WARNING << "Could not find reachability map for kinematic chain '" << setName << "'";
            return false;
        }
    }
 
    VirtualRobot::IKSolver::CartesianSelection RobotIK::convertCartesianSelection(armarx::CartesianSelection cs) const
    {
        switch (cs)
        {
            case armarx::CartesianSelection::eX:
                return VirtualRobot::IKSolver::CartesianSelection::X;
 
            case armarx::CartesianSelection::eY:
                return VirtualRobot::IKSolver::CartesianSelection::Y;
 
            case armarx::CartesianSelection::eZ:
                return VirtualRobot::IKSolver::CartesianSelection::Z;
 
            case armarx::CartesianSelection::ePosition:
                return VirtualRobot::IKSolver::CartesianSelection::Position;
 
            case armarx::CartesianSelection::eOrientation:
                return VirtualRobot::IKSolver::CartesianSelection::Orientation;
 
            case armarx::CartesianSelection::eAll:
                return VirtualRobot::IKSolver::CartesianSelection::All;
        }
 
        return VirtualRobot::IKSolver::CartesianSelection::All;
    }
 
    VirtualRobot::JacobiProvider::InverseJacobiMethod RobotIK::convertInverseJacobiMethod(armarx::InverseJacobiMethod aenum) const
    {
        switch (aenum)
        {
            case armarx::InverseJacobiMethod::eSVD:
                return VirtualRobot::JacobiProvider::InverseJacobiMethod::eSVD;
 
            case armarx::InverseJacobiMethod::eSVDDamped:
                return VirtualRobot::JacobiProvider::InverseJacobiMethod::eSVDDamped;
 
            case armarx::InverseJacobiMethod::eTranspose:
                return VirtualRobot::JacobiProvider::InverseJacobiMethod::eTranspose;
        }
 
        return VirtualRobot::JacobiProvider::InverseJacobiMethod::eSVD;
    }
 
    void RobotIK::synchRobot() const
    {
        std::lock_guard<std::recursive_mutex> lock(_modifyRobotModelMutex);
        RemoteRobot::synchronizeLocalClone(_robotModel, _synchronizedRobot);
    }
 
}