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 <algorithm>
#include <filesystem>
#include <set>
 
#include <VirtualRobot/IK/CoMIK.h>
#include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
#include <VirtualRobot/IK/HierarchicalIK.h>
#include <VirtualRobot/IK/JointLimitAvoidanceJacobi.h>
#include <VirtualRobot/IK/constraints/JointLimitAvoidanceConstraint.h>
#include <VirtualRobot/IK/constraints/OrientationConstraint.h>
#include <VirtualRobot/IK/constraints/PoseConstraint.h>
#include <VirtualRobot/IK/constraints/PositionConstraint.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Workspace/Manipulability.h>
#include <VirtualRobot/Workspace/Reachability.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
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);
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(RobotIK, "RobotIK");
 
} // namespace armarx