KBMComponent.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-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    MemoryX::ArmarXObjects::KBMComponent
 * @author     Jonas Rauber ( kit at jonasrauber dot de )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "KBMComponent.h"
 
#include <cmath>
#include <fstream>
 
#include <VirtualRobot/Import/RobotImporterFactory.h>
#include <VirtualRobot/KinematicChain.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RuntimeEnvironment.h>
 
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <MemoryX/core/entity/EntityAttribute.h>
#include <MemoryX/libraries/memorytypes/entity/KBMData.h>
 
namespace armarx
{
    void
    KBMComponent::onInitComponent()
    {
        usingProxy(getProperty<std::string>("LongtermMemoryName").getValue());
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
    }
 
    void
    KBMComponent::onConnectComponent()
    {
        this->longtermMemoryPrx = getProxy<memoryx::LongtermMemoryInterfacePrx>(
            getProperty<std::string>("LongtermMemoryName").getValue());
        this->robotStateComponentPrx = getProxy<armarx::RobotStateComponentInterfacePrx>(
            getProperty<std::string>("RobotStateComponentName").getValue());
    }
 
    void
    KBMComponent::onDisconnectComponent()
    {
    }
 
    void
    KBMComponent::onExitComponent()
    {
    }
 
    armarx::PropertyDefinitionsPtr
    KBMComponent::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new KBMComponentPropertyDefinitions(getConfigIdentifier()));
    }
 
    /*
     *
     *
     * Helper methods
     *
     *
     */
 
    Eigen::Map<const memoryx::KBM::Vector>
    KBMComponent::mapPosition(const memoryx::KBM::Models::KBM& kbm,
                              const std::vector<memoryx::KBM::Real>& position)
    {
        VR_ASSERT_MESSAGE((int)(position.size()) == kbm.getNDim(),
                          "position size doesn't match KBM dimensions");
        return Eigen::Map<const memoryx::KBM::Vector>(position.data(), kbm.getNDim());
    }
 
    Eigen::Map<const memoryx::KBM::Vector>
    KBMComponent::mapProprioception(const memoryx::KBM::Models::KBM& kbm,
                                    const std::vector<memoryx::KBM::Real>& proprioception)
    {
        VR_ASSERT_MESSAGE((int)(proprioception.size()) == kbm.getNDoF(),
                          "proprioception size doesn't match KBM DoF");
        return Eigen::Map<const memoryx::KBM::Vector>(proprioception.data(), kbm.getNDoF());
    }
 
    Eigen::Map<const memoryx::KBM::Matrix>
    KBMComponent::mapPositions(const memoryx::KBM::Models::KBM& kbm,
                               const std::vector<memoryx::KBM::Real>& position)
    {
        int nDim = kbm.getNDim();
 
        if (position.size() % nDim != 0)
        {
            ARMARX_ERROR << "position has an invalid length";
        }
 
        return mapMatrix(nDim, position);
    }
 
    Eigen::Map<const memoryx::KBM::Matrix>
    KBMComponent::mapProprioceptions(const memoryx::KBM::Models::KBM& kbm,
                                     const std::vector<memoryx::KBM::Real>& proprioception)
    {
        int nDoF = kbm.getNDoF();
 
        if (proprioception.size() % nDoF != 0)
        {
            ARMARX_ERROR << "proprioception has an invalid length";
        }
 
        return mapMatrix(nDoF, proprioception);
    }
 
    Eigen::Map<const memoryx::KBM::Matrix>
    KBMComponent::mapMatrix(int rows, const std::vector<memoryx::KBM::Real>& data)
    {
        if (data.size() % rows != 0)
        {
            ARMARX_ERROR << "data has an invalid length";
        }
 
        VR_ASSERT(data.size() % rows == 0);
        int cols = data.size() / rows;
        return Eigen::Map<const memoryx::KBM::Matrix>(data.data(), rows, cols);
    }
 
    memoryx::KBM::Models::KBM_ptr
    KBMComponent::getKBM(const std::string& name)
    {
        auto it = kbms.find(name);
 
        if (it == kbms.end())
        {
            ARMARX_WARNING << "a KBM with the name '" << name << "' does not exist";
            return memoryx::KBM::Models::KBM_ptr(); // null pointer
        }
 
        return it->second;
    }
 
    /*
     *
     *
     * KBM Component interface methods
     *
     *
     */
 
    void
    KBMComponent::createKBM(const std::string& name,
                            Ice::Int nDoF,
                            Ice::Int dim,
                            Ice::Float spreadAngle,
                            const Ice::Current& c)
    {
        /*
         * Check if a KBM with the given name already exists
         */
        if (kbms.find(name) != kbms.end())
        {
            ARMARX_ERROR << "a KBM with the name '" << name << "' already exists";
            return;
        }
 
        /*
         * Create the KBM
         */
        memoryx::KBM::Models::KBM_ptr kbm(new memoryx::KBM::Models::KBM(nDoF, dim, spreadAngle));
        kbms[name] = kbm;
        createdFromVirtualRobot[name] = false;
        ARMARX_INFO << "created a KBM with the name '" << name << "'";
    }
 
    void
    KBMComponent::createArmar3KBM(const std::string& name,
                                  const Ice::StringSeq& robotNodeNames,
                                  const std::vector<memoryx::KBM::Real>& jointValueAverages,
                                  const std::vector<memoryx::KBM::Real>& spreadAnglesSequence,
                                  const std::string& tcpName,
                                  bool useOrientation,
                                  const Ice::Current& c)
    {
        /*
         * check parameters
         */
        if (robotNodeNames.size() != 7)
        {
            ARMARX_WARNING << "createArmar3KBM should only be used with all 7 joints";
        }
 
        if (jointValueAverages.size() != robotNodeNames.size())
        {
            ARMARX_INFO << "robotNodeNames: " << robotNodeNames.size();
            ARMARX_INFO << "jointValueAverages: " << jointValueAverages.size();
            ARMARX_ERROR << "jointValueAverages should have the same size as robotNodeNames";
        }
 
        if (spreadAnglesSequence.size() != robotNodeNames.size())
        {
            ARMARX_INFO << "robotNodeNames: " << robotNodeNames.size();
            ARMARX_INFO << "spreadAngles: " << spreadAnglesSequence.size();
            ARMARX_ERROR << "spreadAnglesSequence should have the same size as robotNodeNames";
        }
 
        /*
         * Check if a KBM with the given name already exists
         */
        if (kbms.find(name) != kbms.end())
        {
            ARMARX_ERROR << "a KBM with the name '" << name << "' already exists";
            return;
        }
 
        /*
         * spread angles
         */
        Eigen::Map<const memoryx::KBM::Vector> spreadAngles(spreadAnglesSequence.data(),
                                                            spreadAnglesSequence.size());
        ARMARX_IMPORTANT << "spreadAngles: " << spreadAngles.transpose();
 
        /*
         * Save meta data
         */
        std::time_t now = std::time(0);
        std::tm* date = std::localtime(&now);
        char when[256] = {0};
        std::strftime(when, sizeof(when), "%Y-%m-%d-%H-%M-%S", date);
        std::string baseFilename = "data/evaluation/" + std::string(when) + "-" + name;
 
        std::ofstream metaFile(baseFilename + "-create.txt", std::ios_base::app);
        Eigen::Map<const memoryx::KBM::Vector> _jointValueAverages(jointValueAverages.data(),
                                                                   jointValueAverages.size());
 
        if (metaFile.is_open())
        {
            metaFile << "kbm name: " << name << std::endl;
            metaFile << "joint offsets: " << _jointValueAverages << std::endl;
            metaFile << "spread angles: " << spreadAngles << std::endl;
        }
 
        metaFile.close();
 
        /*
         * Load VirtualRobot
         */
        VirtualRobot::RobotImporterFactoryPtr importer =
            VirtualRobot::RobotImporterFactory::fromFileExtension("xml", NULL);
        std::string filename("robots/ArmarIII/ArmarIII.xml");
        VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filename);
        VirtualRobot::RobotPtr robot =
            importer->loadFromFile(filename, VirtualRobot::RobotIO::eStructure);
 
        if (!robot)
        {
            ARMARX_ERROR << "the ArmarIII.xml file could not be loaded";
        }
 
        VR_ASSERT(robot);
 
        /*
         * Create a robot node set with the names of the active joints
         */
        //VirtualRobot::RobotNodeSetPtr nodeSet = robot->getRobotNodeSet("TorsoRightArm");
        VirtualRobot::RobotNodeSetPtr nodeSet = VirtualRobot::RobotNodeSet::createRobotNodeSet(
            robot, "ToolUseRightArm", robotNodeNames, "Right Arm Base", tcpName, true);
 
        if (!nodeSet)
        {
            ARMARX_ERROR << "the nodeSet could not be created";
        }
 
        VR_ASSERT(nodeSet);
 
        /*
         * Create a kinematic chain from the robot node set
         */
        VirtualRobot::KinematicChainPtr chain;
        //chain.reset(new VirtualRobot::KinematicChain("TorsoRightArmChain",robot,nodeSet->getAllRobotNodes(),robot->getRobotNode("Wrist 2 R")));
        chain.reset(new VirtualRobot::KinematicChain("ToolUseRightArmChain",
                                                     robot,
                                                     nodeSet->getAllRobotNodes(),
                                                     robot->getRobotNode(tcpName)));
 
        if (!chain)
        {
            ARMARX_ERROR << "chain could not be created";
        }
 
        VR_ASSERT(chain);
 
        /*
         * Set joint values to their average position,
         * so that samples are distributed around zero
         */
        ARMARX_IMPORTANT << "TCP GlobalPose before setting averages: "
                         << chain->getTCP()->getGlobalPose();
        ARMARX_IMPORTANT << "setting averages: " << jointValueAverages;
        Ice::FloatSeq jointValues;
 
        for (auto& value : jointValueAverages)
        {
            jointValues.push_back(value);
        }
 
        chain->setJointValues(jointValues);
        ARMARX_VERBOSE << "node " << chain->getNode(0)->getName()
                       << " is at: " << chain->getNode(0)->getJointValue();
        ARMARX_IMPORTANT << "TCP GlobalPose after setting averages: "
                         << chain->getTCP()->getGlobalPose();
 
        VirtualRobot::RobotNodePtr FoR = chain->getKinematicRoot();
 
        VR_ASSERT(FoR);
 
        /*
         * TODO: print warning if not
         */
        ARMARX_IMPORTANT << "FoR GlobalPose should be the identity matrix: "
                         << FoR->getGlobalPose();
 
        unsigned int nDoF = chain->getSize();
        ARMARX_IMPORTANT << "degrees of freedom of the VirtualRobot's kinematic chain: " << nDoF;
 
        /*
         * Create the KBM
         */
        memoryx::KBM::Models::KBM_ptr kbm(memoryx::KBM::Models::KBM::createFromVirtualRobot(
            chain, FoR, spreadAngles, useOrientation));
        kbms[name] = kbm;
        createdFromVirtualRobot[name] = true;
        ARMARX_INFO << "created a KBM with the name '" << name << "'";
    }
 
    void
    KBMComponent::batch(const std::string& name,
                        const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
                        const std::vector<memoryx::KBM::Real>& positionSequence,
                        Ice::Int method,
                        Ice::Float threshold,
                        const Ice::Current& c)
    {
        memoryx::KBM::Models::KBM_ptr kbm = getKBM(name);
 
        if (!kbm)
        {
            return;
        }
 
        /*
         * It doesn't make sense to do batch learning on KBMs created from a VirtualRobot.
         */
        if (createdFromVirtualRobot[name])
        {
            ARMARX_ERROR << "the KBM " << name
                         << " was created from a VirtualRobot, it does not support batch learning";
            return;
        }
 
        /*
         * This maps the data column wise.
         */
        Eigen::Map<const memoryx::KBM::Matrix> proprioception =
            mapProprioceptions(*kbm, proprioceptionSequence);
        Eigen::Map<const memoryx::KBM::Matrix> position = mapPositions(*kbm, positionSequence);
 
        if (proprioception.cols() != position.cols())
        {
            ARMARX_ERROR << "proprioception and position contain different number of samples";
            return;
        }
 
        /*
         * Start batch learning
         */
        kbm->batch(
            proprioception, position, (memoryx::KBM::Models::KBM::Optimization)method, threshold);
    }
 
    void
    KBMComponent::online(const std::string& name,
                         const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
                         const std::vector<memoryx::KBM::Real>& positionSequence,
                         Ice::Float learningRate,
                         const Ice::Current& c)
    {
        memoryx::KBM::Models::KBM_ptr kbm = getKBM(name);
 
        if (!kbm)
        {
            return;
        }
 
        /*
         * This maps the data column wise.
         */
        Eigen::Map<const memoryx::KBM::Matrix> proprioception =
            mapProprioceptions(*kbm, proprioceptionSequence);
        Eigen::Map<const memoryx::KBM::Matrix> position = mapPositions(*kbm, positionSequence);
 
        if (proprioception.cols() != position.cols())
        {
            ARMARX_ERROR << "proprioception and position contain different number of samples";
            return;
        }
 
        if (createdFromVirtualRobot[name])
        {
            /*
             * support VirtualRobot KBMs by reversing joint order
             */
            ARMARX_IMPORTANT
                << "reversing proprioception order because KBM was created from VirtualRobot";
            kbm->online(proprioception.colwise().reverse(), position, learningRate);
        }
        else
        {
            // TODO: why does this work: KBM expects a Matrix and should not be
            // compatible with a Map of Matrix without additional modifications
            kbm->online(proprioception, position, learningRate);
        }
    }
 
    void
    KBMComponent::onlineVerbose(
        const std::string& name,
        const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
        const std::vector<memoryx::KBM::Real>& positionSequence,
        Ice::Float learningRate,
        const std::vector<memoryx::KBM::Real>& evaluationProprioceptionSequence,
        const std::vector<memoryx::KBM::Real>& evaluationPositionSequence,
        const Ice::Current& c)
    {
        memoryx::KBM::Models::KBM_ptr kbm = getKBM(name);
 
        if (!kbm)
        {
            return;
        }
 
        /*
         * This maps the data column wise.
         */
        Eigen::Map<const memoryx::KBM::Matrix> _proprioception =
            mapProprioceptions(*kbm, proprioceptionSequence);
        Eigen::Map<const memoryx::KBM::Matrix> position = mapPositions(*kbm, positionSequence);
 
        Eigen::Map<const memoryx::KBM::Matrix> _evaluationProprioception =
            mapProprioceptions(*kbm, evaluationProprioceptionSequence);
        Eigen::Map<const memoryx::KBM::Matrix> evaluationPosition =
            mapPositions(*kbm, evaluationPositionSequence);
 
        if (_proprioception.cols() != position.cols())
        {
            ARMARX_ERROR << "proprioception and position contain different number of samples";
            return;
        }
 
        if (_evaluationProprioception.cols() != evaluationPosition.cols())
        {
            ARMARX_ERROR << "evaluation proprioception and evaluation position contain different "
                            "number of samples";
            return;
        }
 
        memoryx::KBM::Matrix proprioception;
        memoryx::KBM::Matrix evaluationProprioception;
 
        if (createdFromVirtualRobot[name])
        {
            /*
             * support VirtualRobot KBMs by reversing joint order
             */
            ARMARX_IMPORTANT
                << "reversing proprioception order because KBM was created from VirtualRobot";
            proprioception = _proprioception.colwise().reverse();
            evaluationProprioception = _evaluationProprioception.colwise().reverse();
        }
        else
        {
            proprioception = _proprioception;
            evaluationProprioception = _evaluationProprioception;
        }
 
        ARMARX_IMPORTANT << "training samples: " << proprioception.cols();
        ARMARX_IMPORTANT << "evaluation samples: " << evaluationProprioception.cols();
 
        /*
         * Prepare file
         */
 
        std::time_t now = std::time(0);
        std::tm* date = std::localtime(&now);
        char when[256] = {0};
        std::strftime(when, sizeof(when), "%Y-%m-%d-%H-%M-%S", date);
        std::string baseFilename = "data/evaluation/" + std::string(when) + "-" + name;
 
        std::ofstream metaFile(baseFilename + "-meta.txt", std::ios_base::app);
        std::string evaluationFilename(baseFilename + "-evaluation.txt");
        std::ofstream evaluationFile(evaluationFilename, std::ios_base::app);
        std::string trainingProprioceptionsFilename(baseFilename + "-training-proprioceptions.txt");
        std::ofstream trainingProprioceptionsFile(trainingProprioceptionsFilename,
                                                  std::ios_base::app);
        std::string trainingPositionsFilename(baseFilename + "-training-positions.txt");
        std::ofstream trainingPositionsFile(trainingPositionsFilename, std::ios_base::app);
        std::string testProprioceptionsFilename(baseFilename + "-test-proprioceptions.txt");
        std::ofstream testProprioceptionsFile(testProprioceptionsFilename, std::ios_base::app);
        std::string testPositionsFilename(baseFilename + "-test-positions.txt");
        std::ofstream testPositionsFile(testPositionsFilename, std::ios_base::app);
 
        if (metaFile.is_open())
        {
            metaFile << "kbm name: " << name << std::endl;
            metaFile << "learning rate: " << learningRate << std::endl;
            metaFile << "degress of freedom: " << kbm->getNDoF() << std::endl;
            metaFile << "dimensions: " << kbm->getNDim() << std::endl;
            metaFile << "training samples: " << proprioception.cols() << std::endl;
            metaFile << "test samples: " << evaluationProprioception.cols() << std::endl;
            metaFile << "evaluation columns: mean, std, median, iqr, max" << std::endl;
        }
 
        metaFile.close();
 
        if (!trainingProprioceptionsFile.is_open())
        {
            ARMARX_ERROR << "file " << trainingProprioceptionsFilename
                         << " could not be opened / created";
        }
 
        if (!trainingPositionsFile.is_open())
        {
            ARMARX_ERROR << "file " << trainingPositionsFilename
                         << " could not be opened / created";
        }
 
        if (!testProprioceptionsFile.is_open())
        {
            ARMARX_ERROR << "file " << testProprioceptionsFilename
                         << " could not be opened / created";
        }
 
        if (!testPositionsFile.is_open())
        {
            ARMARX_ERROR << "file " << testPositionsFilename << " could not be opened / created";
        }
 
        trainingProprioceptionsFile << proprioception.transpose() << std::endl;
        trainingPositionsFile << position.transpose() << std::endl;
        testProprioceptionsFile << evaluationProprioception.transpose() << std::endl;
        testPositionsFile << evaluationPosition.transpose() << std::endl;
 
        trainingProprioceptionsFile.close();
        trainingPositionsFile.close();
        testProprioceptionsFile.close();
        testPositionsFile.close();
 
        /*
         * Learn, evaluate and write to file
         */
        if (!evaluationFile.is_open())
        {
            ARMARX_ERROR << "file " << evaluationFilename << " could not be opened / created";
        }
 
        memoryx::KBM::Models::KBM::ErrorValuesType errors =
            kbm->getErrors(evaluationProprioception, evaluationPosition);
        ARMARX_IMPORTANT << "online learning before learning: " << errors;
        evaluationFile << errors.Mean << ", " << errors.STD << ", " << errors.Median << ", "
                       << errors.IQR << ", " << errors.Max << std::endl;
 
        for (int i = 0; i < proprioception.cols(); i++)
        {
            kbm->online(proprioception.col(i), position.col(i), learningRate);
            /*
             * TODO: maybe use my own error calculation, including standard deviation, etc.
             */
            errors = kbm->getErrors(evaluationProprioception, evaluationPosition);
            ARMARX_IMPORTANT << "online learning error after step " << (i + 1) << ": " << errors;
            evaluationFile << errors.Mean << ", " << errors.STD << ", " << errors.Median << ", "
                           << errors.IQR << ", " << errors.Max << std::endl;
        }
 
        evaluationFile.close();
    }
 
    std::vector<memoryx::KBM::Real>
    KBMComponent::predict(const std::string& name,
                          const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
                          const Ice::Current& c)
    {
        memoryx::KBM::Models::KBM_ptr kbm = getKBM(name);
 
        if (!kbm)
        {
            return std::vector<memoryx::KBM::Real>();
        }
 
        Eigen::Map<const memoryx::KBM::Matrix> proprioception =
            mapProprioceptions(*kbm, proprioceptionSequence);
 
        int nDim = kbm->getNDim();
        int nSamples = proprioception.cols();
        std::vector<memoryx::KBM::Real> predictionSequence(nDim * nSamples);
        Eigen::Map<memoryx::KBM::Matrix> prediction(predictionSequence.data(), nDim, nSamples);
 
        if (createdFromVirtualRobot[name])
        {
            /*
             * support VirtualRobot KBMs by reversing joint order
             */
            prediction = kbm->predict(proprioception.colwise().reverse());
        }
        else
        {
            prediction = kbm->predict(proprioception);
        }
 
        ARMARX_INFO << "KBM '" << name << "' predicted: " << prediction.transpose();
        return predictionSequence;
    }
 
    void
    KBMComponent::printErrors(const std::string& name,
                              const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
                              const std::vector<memoryx::KBM::Real>& positionSequence,
                              const Ice::Current& c)
    {
        memoryx::KBM::Models::KBM_ptr kbm = getKBM(name);
 
        if (!kbm)
        {
            return;
        }
 
        /*
         * This maps the data column wise.
         */
        Eigen::Map<const memoryx::KBM::Matrix> proprioception =
            mapProprioceptions(*kbm, proprioceptionSequence);
        Eigen::Map<const memoryx::KBM::Matrix> position = mapPositions(*kbm, positionSequence);
 
        if (proprioception.cols() != position.cols())
        {
            ARMARX_ERROR << "proprioception and position contain different number of samples";
            return;
        }
 
        ARMARX_IMPORTANT << "samples: " << proprioception.cols();
 
        memoryx::KBM::Models::KBM::ErrorValuesType errors;
 
        if (createdFromVirtualRobot[name])
        {
            /*
             * support VirtualRobot KBMs by reversing joint order
             */
            errors = kbm->getErrors(proprioception.colwise().reverse(), position);
        }
        else
        {
            errors = kbm->getErrors(proprioception, position);
        }
 
        ARMARX_IMPORTANT << "KBM '" << name << "' error: " << errors;
    }
 
    void
    KBMComponent::printAccumulatorStatistics(int nDim, int nDoF, const Ice::Current& c)
    {
        Eigen::Map<const memoryx::KBM::Matrix> proprioception =
            mapMatrix(nDoF, proprioceptionAccumulator);
        Eigen::Map<const memoryx::KBM::Matrix> position = mapMatrix(nDim, positionAccumulator);
 
        ARMARX_VERBOSE << "proprioception samples: " << proprioception.cols();
        ARMARX_VERBOSE << "position samples: " << position.cols();
 
        ARMARX_VERBOSE << "proprioception minima: "
                       << proprioception.rowwise().minCoeff().transpose();
        ARMARX_VERBOSE << "proprioception maxima: "
                       << proprioception.rowwise().maxCoeff().transpose();
        ARMARX_VERBOSE << "proprioception average: " << proprioception.rowwise().mean().transpose();
        ARMARX_VERBOSE << "position minima: " << position.rowwise().minCoeff().transpose();
        ARMARX_VERBOSE << "position maxima: " << position.rowwise().maxCoeff().transpose();
        ARMARX_VERBOSE << "position average: " << position.rowwise().mean().transpose();
    }
 
    Ice::DoubleSeq
    KBMComponent::getRawJointValuesAverages(int nDoF, const Ice::Current& c)
    {
        Eigen::Map<const memoryx::KBM::Matrix> rawJointValues =
            mapMatrix(nDoF, rawJointValuesAccumulator);
 
        ARMARX_VERBOSE << "raw jointValue samples: " << rawJointValues.cols();
 
        std::vector<memoryx::KBM::Real> result(nDoF);
        Eigen::Map<memoryx::KBM::Vector> averages(result.data(), nDoF);
        averages = rawJointValues.rowwise().mean();
 
        Ice::DoubleSeq resultFloat(result.begin(), result.end());
        return resultFloat;
    }
 
    Ice::DoubleSeq
    KBMComponent::getSpreadAngles(int nDoF, const Ice::Current& c)
    {
        Eigen::Map<const memoryx::KBM::Matrix> proprioceptions =
            mapMatrix(nDoF, proprioceptionAccumulator);
 
        ARMARX_VERBOSE << "proprioception samples: " << proprioceptions.cols();
 
        std::vector<memoryx::KBM::Real> result(nDoF);
        Eigen::Map<memoryx::KBM::Vector> spreadAngles(result.data(), nDoF);
 
        memoryx::KBM::Vector leftSpreadAngles = proprioceptions.rowwise().minCoeff();
        memoryx::KBM::Vector rightSpreadAngles = proprioceptions.rowwise().maxCoeff();
        ARMARX_VERBOSE << "leftSpreadAngles: " << leftSpreadAngles;
        ARMARX_VERBOSE << "rightSpreadAngles: " << rightSpreadAngles;
 
        for (int i = 0; i < nDoF; i++)
        {
            spreadAngles[i] = fmin(fabs(leftSpreadAngles[i]), fabs(rightSpreadAngles[i]));
        }
 
        ARMARX_VERBOSE << "data spreadAngles: " << spreadAngles;
 
        Ice::DoubleSeq resultFloat(result.begin(), result.end());
        return resultFloat;
    }
 
    void
    KBMComponent::setProprioceptionAccumulatorFromRawJointAccumulator(
        const std::vector<memoryx::KBM::Real>& rawJointValuesAverages,
        const Ice::Current& c)
    {
        if (proprioceptionAccumulator.size() != 0)
        {
            ARMARX_ERROR << "proprioception accumulator should be empty, when using setting it "
                            "from raw joint accumulator";
        }
 
        if (rawJointValuesAccumulator.size() % rawJointValuesAverages.size() != 0)
        {
            ARMARX_ERROR << "rawJointValuesAverages size is incompatible to accumulator size";
        }
 
        ARMARX_INFO << "rawJointValuesAccumualtor: " << rawJointValuesAccumulator;
        std::vector<memoryx::KBM::Real> proprioceptionsFromRawJointAccumulator;
        unsigned int i = 0;
 
        for (auto jointValue : rawJointValuesAccumulator)
        {
            if (i == rawJointValuesAverages.size())
            {
                i = 0;
            }
 
            float offset = rawJointValuesAverages[i];
            i++;
 
            /*
             * Convert to proprioception
             */
            proprioceptionsFromRawJointAccumulator.push_back(jointValue - offset);
        }
 
        ARMARX_INFO << "proprioceptionsFromRawJointAccumulator: "
                    << proprioceptionsFromRawJointAccumulator;
        proprioceptionAccumulator = proprioceptionsFromRawJointAccumulator;
 
        /*
         * TODO: move to separate method
         */
    }
 
    void
    KBMComponent::setPositionLimits(int nDim, const Ice::Current& c)
    {
        Eigen::Map<const memoryx::KBM::Matrix> positions = mapMatrix(nDim, positionAccumulator);
        minima = positions.rowwise().minCoeff();
        maxima = positions.rowwise().maxCoeff();
        ARMARX_IMPORTANT << "setting position minima: " << minima.transpose();
        ARMARX_IMPORTANT << "setting position maxima: " << maxima.transpose();
    }
 
    void
    KBMComponent::onlineAccumulator(const std::string& name,
                                    Ice::Float learningRate,
                                    const Ice::Current& c)
    {
        online(name, proprioceptionAccumulator, positionAccumulator, learningRate, c);
    }
 
    void
    KBMComponent::onlineAccumulatorVerbose(const std::string& name,
                                           Ice::Float learningRate,
                                           const Ice::Current& c)
    {
        onlineVerbose(name,
                      proprioceptionAccumulator,
                      positionAccumulator,
                      learningRate,
                      evaluationProprioceptionAccumulator,
                      evaluationPositionAccumulator,
                      c);
    }
 
    void
    KBMComponent::batchAccumulator(const std::string& name,
                                   Ice::Int method,
                                   Ice::Float threshold,
                                   const Ice::Current& c)
    {
        batch(name, proprioceptionAccumulator, positionAccumulator, method, threshold, c);
    }
 
    void
    KBMComponent::printErrorsAccumulator(const std::string& name, const Ice::Current& c)
    {
        printErrors(name, proprioceptionAccumulator, positionAccumulator, c);
    }
 
    void
    KBMComponent::predictAccumulator(const std::string& name, const Ice::Current& c)
    {
        predict(name, proprioceptionAccumulator, c);
    }
 
    /*
     *
     *
     * The following methods should alredady have orientation support
     *
     *
     */
 
    void
    KBMComponent::accumulatePositions(const std::vector<memoryx::KBM::Real>& position,
                                      const Ice::Current& c)
    {
        /*
         * accumulate positions
         */
        positionAccumulator.insert(positionAccumulator.end(), position.begin(), position.end());
 
        //ARMARX_VERBOSE << "Positions Accumulator: " << positionAccumulator;
    }
 
    void
    KBMComponent::accumulateProprioceptions(const std::vector<memoryx::KBM::Real>& proprioception,
                                            const Ice::Current& c)
    {
        /*
         * accumulate proprioceptions
         */
        proprioceptionAccumulator.insert(
            proprioceptionAccumulator.end(), proprioception.begin(), proprioception.end());
 
        //ARMARX_VERBOSE << "Proprioception Accumulator: " << proprioceptionAccumulator;
    }
 
    void
    KBMComponent::accumulateRawJointValues(
        const std::vector<memoryx::KBM::Real>& rawJointValuesSequence,
        const Ice::Current& c)
    {
        /*
         * accumulate raw joint values
         */
        rawJointValuesAccumulator.insert(rawJointValuesAccumulator.end(),
                                         rawJointValuesSequence.begin(),
                                         rawJointValuesSequence.end());
 
        //ARMARX_VERBOSE << "Raw Joint Values Accumulator: " << rawJointValuesAccumulator;
    }
 
    void
    KBMComponent::accumulateEvaluationPositions(const std::vector<memoryx::KBM::Real>& position,
                                                const Ice::Current& c)
    {
        /*
         * accumulate positions
         */
        evaluationPositionAccumulator.insert(
            evaluationPositionAccumulator.end(), position.begin(), position.end());
 
        //ARMARX_VERBOSE << "Positions Accumulator: " << positionAccumulator;
    }
 
    void
    KBMComponent::accumulateEvaluationProprioceptions(
        const std::vector<memoryx::KBM::Real>& proprioception,
        const Ice::Current& c)
    {
        /*
         * accumulate proprioceptions
         */
        evaluationProprioceptionAccumulator.insert(evaluationProprioceptionAccumulator.end(),
                                                   proprioception.begin(),
                                                   proprioception.end());
 
        //ARMARX_VERBOSE << "Proprioception Accumulator: " << proprioceptionAccumulator;
    }
 
    std::vector<memoryx::KBM::Real>
    KBMComponent::solveGlobalIK(const std::string& name,
                                const std::vector<memoryx::KBM::Real>& targetPosition,
                                const Ice::Current& c)
    {
        /*
         * Check input variables
         */
        memoryx::KBM::Models::KBM_ptr kbm = getKBM(name);
 
        if (!kbm)
        {
            ARMARX_WARNING << "returning empty solution";
            return std::vector<memoryx::KBM::Real>();
        }
 
        Eigen::Map<const memoryx::KBM::Vector> target(mapPosition(*kbm, targetPosition));
 
        /*
         * Create output variables
         */
        std::vector<memoryx::KBM::Real> result(kbm->getNDoF());
        Eigen::Map<memoryx::KBM::Vector> solution(result.data(), kbm->getNDoF());
 
        /*
         * Solve Global IK
         */
        memoryx::KBM::Inverse::SolutionSet solutions(
            memoryx::KBM::Inverse::solveGlobalIK(kbm, target, target));
        ARMARX_IMPORTANT << "Global IK found " << solutions.size() << " solutions";
 
        for (unsigned int i = 0; i < solutions.size(); ++i)
        {
            memoryx::KBM::Inverse::Solution solution = solutions[i];
            ARMARX_IMPORTANT << i << ". solution: " << solution.spreadAngles;
        }
 
        // TODO: support VirtualRobot KBMs by switching proprioceptions
        if (createdFromVirtualRobot[name])
        {
            ARMARX_ERROR << "solveGlobalIK does not yet support KBMs created from Virtual Robot!";
        }
 
        return result; // TODO: currently always 0
    }
 
    std::vector<memoryx::KBM::Real>
    KBMComponent::solveDifferentialIK(
        const std::string& name,
        const std::vector<memoryx::KBM::Real>& targetPosition,
        const std::vector<memoryx::KBM::Real>& currentPosition,
        const std::vector<memoryx::KBM::Real>& currentProprioception,
        const std::vector<memoryx::KBM::Real>& lowerProprioceptionLimitsSequence,
        const std::vector<memoryx::KBM::Real>& upperProprioceptionLimitsSequence,
        const Ice::Current& c)
    {
        /*
         * Check input variables
         */
        memoryx::KBM::Models::KBM_ptr kbm = getKBM(name);
 
        if (!kbm)
        {
            ARMARX_WARNING << "returning empty solution";
            return std::vector<memoryx::KBM::Real>();
        }
 
        Eigen::Map<const memoryx::KBM::Vector> target(mapPosition(*kbm, targetPosition));
        Eigen::Map<const memoryx::KBM::Vector> current(mapPosition(*kbm, currentPosition));
        Eigen::Map<const memoryx::KBM::Vector> proprioception(
            mapProprioception(*kbm, currentProprioception));
        Eigen::Map<const memoryx::KBM::Vector> lowerProprioceptionLimits(
            mapProprioception(*kbm, lowerProprioceptionLimitsSequence));
        Eigen::Map<const memoryx::KBM::Vector> upperProprioceptionLimits(
            mapProprioception(*kbm, upperProprioceptionLimitsSequence));
 
        /*
         * Check position limits
         */
        if (minima.size() != target.size())
        {
            ARMARX_ERROR << "minima and target have different size";
            return std::vector<memoryx::KBM::Real>();
        }
 
        if (maxima.size() != target.size())
        {
 
            ARMARX_ERROR << "maxima and target have different size";
            return std::vector<memoryx::KBM::Real>();
        }
 
        for (int i = 0; i < kbm->getNDim(); i++)
        {
            if (target[i] < minima[i] - 90)
            {
                ARMARX_ERROR << "target[" << i << "] = " << target[i] << " is smaller than minima "
                             << minima[i];
                return std::vector<memoryx::KBM::Real>();
            }
 
            if (target[i] > maxima[i] + 90)
            {
                ARMARX_ERROR << "target[" << i << "] = " << target[i] << " is greater than maxima "
                             << maxima[i];
                return std::vector<memoryx::KBM::Real>();
            }
        }
 
        /*
         * Create output variables
         */
        std::vector<memoryx::KBM::Real> result(kbm->getNDoF());
        Eigen::Map<memoryx::KBM::Vector> solution(result.data(), kbm->getNDoF());
 
        /*
         * Solve Differential IK
         */
        bool solved;
 
        if (createdFromVirtualRobot[name])
        {
            /*
             * support VirtualRobot KBMs by reversing joint order
             */
            ARMARX_IMPORTANT << "reversing proprioception and limits order because KBM was created "
                                "from VirtualRobot";
            solved = KBMDifferentialIK::solveMany(*kbm,
                                                  target,
                                                  current,
                                                  proprioception.reverse(),
                                                  lowerProprioceptionLimits.reverse(),
                                                  upperProprioceptionLimits.reverse(),
                                                  solution);
        }
        else
        {
            solved = KBMDifferentialIK::solveMany(*kbm,
                                                  target,
                                                  current,
                                                  proprioception,
                                                  lowerProprioceptionLimits,
                                                  upperProprioceptionLimits,
                                                  solution);
        }
 
        if (createdFromVirtualRobot[name])
        {
            /*
             * support VirtualRobot KBMs by reversing joint order
             */
            ARMARX_IMPORTANT << "solution before reverse: " << solution.transpose();
            solution = solution.colwise().reverse();
            ARMARX_IMPORTANT << "solution after reverse: " << solution.transpose();
        }
 
        /*
         * Output
         */
        ARMARX_IMPORTANT << "solved = " << solved;
        ARMARX_IMPORTANT << "solution = " << solution;
        ARMARX_IMPORTANT << "result (check if equal to solution!) = " << result;
 
        return result;
    }
 
    /*
     *
     *
     * KBM Differential Inverse Kinematics
     *
     *
     */
 
    float
    KBMDifferentialIK::randomFloat(float LO, float HI)
    {
        return LO + static_cast<float>(rand()) / (static_cast<float>(RAND_MAX / (HI - LO)));
    }
 
    memoryx::KBM::Vector
    KBMDifferentialIK::calculateJointDeltas(const memoryx::KBM::Models::KBM& kbm,
                                            const memoryx::KBM::Vector& target,
                                            const memoryx::KBM::Vector& position,
                                            const memoryx::KBM::Vector& proprioception,
                                            float stepSizeFactor,
                                            float maxStepSize)
    {
 
        /*
         * Calculate postion deltas
         */
        memoryx::KBM::Vector positionDeltas = stepSizeFactor * (target - position);
 
        if (positionDeltas.norm() > maxStepSize)
        {
            positionDeltas *= maxStepSize / positionDeltas.norm();
        }
 
        /*
         * Calcualte the jacobian matrix
         */
        memoryx::KBM::Matrix jacobian = kbm.getJacobian(proprioception);
 
        /*
         * Calculate pseudo inverse using function from simox
         * simox uses an algorithm that is numerically stable
         */
#ifdef KBM_USE_DOUBLE_PRECISION
        memoryx::KBM::Matrix pseudoInverse = VirtualRobot::MathTools::getPseudoInverseD(jacobian);
#else
        memoryx::KBM::Matrix pseudoInverse = VirtualRobot::MathTools::getPseudoInverse(jacobian);
#endif
 
        /*
         * Calculate joint deltas
         */
        memoryx::KBM::Vector jointDeltas = pseudoInverse * positionDeltas;
        return jointDeltas;
    }
 
    memoryx::KBM::Vector
    KBMDifferentialIK::randomProprioception(const memoryx::KBM::Vector& lowerProprioceptionLimits,
                                            const memoryx::KBM::Vector& upperProprioceptionLimits)
    {
        if (lowerProprioceptionLimits.size() != upperProprioceptionLimits.size())
        {
            ARMARX_ERROR_S << "limits must have same length";
        }
 
        int n = lowerProprioceptionLimits.size();
        memoryx::KBM::Vector result(n);
 
        for (int i = 0; i < n; i++)
        {
            float l = lowerProprioceptionLimits(i);
            float u = upperProprioceptionLimits(i);
            result(i) = randomFloat(l, u);
        }
 
        return result;
    }
 
    bool
    KBMDifferentialIK::checkProprioceptionLimits(
        const memoryx::KBM::Vector& solution,
        const memoryx::KBM::Vector& lowerProprioceptionLimits,
        const memoryx::KBM::Vector& upperProprioceptionLimits)
    {
        /*
         * Check parameters
         */
        int n = solution.size();
 
        if (lowerProprioceptionLimits.size() != n)
        {
            ARMARX_ERROR_S << "lower proprioception limits have wrong length: "
                           << lowerProprioceptionLimits.size() << ", should be " << n;
        }
 
        if (upperProprioceptionLimits.size() != n)
        {
            ARMARX_ERROR_S << "upper proprioception limits have wrong length: "
                           << upperProprioceptionLimits.size() << ", should be " << n;
        }
 
        /*
         * Check limits
         */
        for (int i = 0; i < n; i++)
        {
            if ((solution(i) < lowerProprioceptionLimits(i)) ||
                (solution(i) > upperProprioceptionLimits(i)))
            {
                return false;
            }
        }
 
        return true;
    }
 
    bool
    KBMDifferentialIK::applyProprioceptionLimits(
        Eigen::Map<memoryx::KBM::Vector>& solution,
        const memoryx::KBM::Vector& lowerProprioceptionLimits,
        const memoryx::KBM::Vector& upperProprioceptionLimits)
    {
        /*
         * Check parameters
         */
        int n = solution.size();
 
        if (lowerProprioceptionLimits.size() != n)
        {
            ARMARX_ERROR_S << "lower proprioception limits have wrong length: "
                           << lowerProprioceptionLimits.size() << ", should be " << n;
        }
 
        if (upperProprioceptionLimits.size() != n)
        {
            ARMARX_ERROR_S << "upper proprioception limits have wrong length: "
                           << upperProprioceptionLimits.size() << ", should be " << n;
        }
 
        /*
         * Check limits
         */
        bool inLimits = true;
 
        for (int i = 0; i < n; i++)
        {
            if (solution(i) < lowerProprioceptionLimits(i))
            {
                solution(i) = lowerProprioceptionLimits(i);
                inLimits = false;
            }
 
            if (solution(i) > upperProprioceptionLimits(i))
            {
                solution(i) = upperProprioceptionLimits(i);
                inLimits = false;
            }
        }
 
        return inLimits;
    }
 
    bool
    KBMDifferentialIK::solveMany(const memoryx::KBM::Models::KBM& kbm,
                                 const memoryx::KBM::Vector& targetPosition,
                                 const memoryx::KBM::Vector& currentPosition,
                                 const memoryx::KBM::Vector& currentProprioception,
                                 const memoryx::KBM::Vector& lowerProprioceptionLimits,
                                 const memoryx::KBM::Vector& upperProprioceptionLimits,
                                 Eigen::Map<memoryx::KBM::Vector>& solution,
                                 bool applyLimits,
                                 int maxSolves,
                                 float positionTolerance,
                                 float minimumDelta,
                                 bool requireImprovment,
                                 int maxSteps,
                                 float stepSizeFactor,
                                 float maxStepSize)
    {
 
        float bestDistance = FLT_MAX;
        bool inLimits = false;
 
        std::vector<memoryx::KBM::Real> newSolutionSequence(kbm.getNDoF());
        Eigen::Map<memoryx::KBM::Vector> newSolution(newSolutionSequence.data(), kbm.getNDoF());
 
        for (int i = 0; i < maxSolves; i++)
        {
            memoryx::KBM::Vector startProprioception;
 
            if (i == 0)
            {
                startProprioception = currentProprioception;
            }
            else
            {
                startProprioception =
                    randomProprioception(lowerProprioceptionLimits, upperProprioceptionLimits);
            }
 
            ARMARX_IMPORTANT_S << "iteration " << (i + 1)
                               << " proprioception: " << startProprioception.transpose();
            memoryx::KBM::Vector startPosition = kbm.predict(startProprioception);
 
            /*
             * TODO: it's probably much more efficient, to use the limits
             * in the solve method itself and consider the limits right away.
             */
            KBMDifferentialIK::solve(kbm,
                                     targetPosition,
                                     startPosition,
                                     startProprioception,
                                     newSolution,
                                     positionTolerance,
                                     minimumDelta,
                                     requireImprovment,
                                     maxSteps,
                                     stepSizeFactor,
                                     maxStepSize);
 
            /*
             * When using applyLimits, all solutions will be in limits, but solutions
             * that were outside the limits will probably get worse.
             *
             * Note: when using this option, the following limits check and solution ignoring
             * doesn't have any effect.
             */
            if (applyLimits)
            {
                /*
                 * distance before enforcing limits
                 */
                memoryx::KBM::Vector testPosition = kbm.predict(newSolution);
                float distance = (targetPosition - testPosition).norm();
                ARMARX_INFO_S << "distance before enforcing limits: " << distance;
 
                /*
                 * modify the solution to be within the limits
                 */
                ARMARX_VERBOSE_S << "solution before apply limits: " << newSolution;
                bool wasInLimits = applyProprioceptionLimits(
                    newSolution, lowerProprioceptionLimits, upperProprioceptionLimits);
                ARMARX_VERBOSE_S << "solution after apply limits: " << newSolution;
 
                if (!wasInLimits)
                {
                    ARMARX_IMPORTANT_S
                        << "solution was not within limits, but enforced limits so now it is";
                }
 
                /*
                 * distance after enforcing limits
                 */
                testPosition = kbm.predict(newSolution);
                distance = (targetPosition - testPosition).norm();
                ARMARX_INFO_S << "distance after enforcing limits: " << distance;
            }
 
            /*
             * If we already found a solution in joint limits,
             * only accept other solutions in joint limits.
             */
            bool currentInLimits = checkProprioceptionLimits(
                newSolution, lowerProprioceptionLimits, upperProprioceptionLimits);
            ARMARX_INFO_S << "is in limits: " << currentInLimits;
 
            if (applyLimits && !currentInLimits)
            {
                ARMARX_ERROR_S << "this should not be possible";
                ARMARX_ERROR_S << "problematic solution: " << newSolution;
                ARMARX_ERROR_S << "lower proprioception limits: " << lowerProprioceptionLimits;
                ARMARX_ERROR_S << "upper proprioception limits: " << upperProprioceptionLimits;
            }
 
            if (inLimits && !currentInLimits)
            {
                ARMARX_IMPORTANT_S
                    << "ignoring solution because we already found one that is within the limits";
                continue;
            }
 
            memoryx::KBM::Vector newPosition = kbm.predict(newSolution);
            float distance = (targetPosition - newPosition).norm();
 
            ARMARX_INFO_S << "distance to target: " << distance << ", best known distance "
                          << bestDistance;
 
            if ((distance < bestDistance) || (currentInLimits && !inLimits))
            {
                bestDistance = distance;
                inLimits = currentInLimits;
                solution = newSolution;
                ARMARX_IMPORTANT_S << "found new solution: " << solution.transpose();
 
                if (inLimits && (bestDistance < positionTolerance))
                {
                    ARMARX_IMPORTANT_S << "found a solution within the limits with small distance, "
                                          "aborting search";
                    break;
                }
            }
        }
 
        ARMARX_IMPORTANT_S << "final distance: " << bestDistance << ", in limits: " << inLimits;
 
        if (bestDistance < positionTolerance)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
 
    bool
    KBMDifferentialIK::solve(const memoryx::KBM::Models::KBM& kbm,
                             const memoryx::KBM::Vector& targetPosition,
                             const memoryx::KBM::Vector& currentPosition,
                             const memoryx::KBM::Vector& currentProprioception,
                             Eigen::Map<memoryx::KBM::Vector>& solution,
                             float positionTolerance,
                             float minimumDelta,
                             bool requireImprovment,
                             int maxSteps,
                             float stepSizeFactor,
                             float maxStepSize)
    {
 
        assert(targetPosition.size() == kbm.getNDim());
        assert(currentPosition.size() == kbm.getNDim());
        assert(currentProprioception.size() == kbm.getNDoF());
        assert(solution.size() == kbm.getNDoF());
 
        /*
         * In the beginning, our best guess it the current proprioception
         */
        solution = currentProprioception;
 
        /*
         * The best known distance
         */
        float bestDistance = (targetPosition - currentPosition).norm();
 
        /*
         * Check if the initial position is already a solution
         */
        if (bestDistance <= positionTolerance)
        {
            return true;
        }
 
        /*
         * Current joint values
         */
        memoryx::KBM::Vector simulatedProprioception(currentProprioception);
 
        /*
         * Current position
         */
        memoryx::KBM::Vector predictedPosition(currentPosition);
 
        /*
         * Compare this to the predicted position
         */
        ARMARX_IMPORTANT_S << "current predicted position: " << kbm.predict(currentProprioception);
 
        for (int step = 0; step < maxSteps; ++step)
        {
            /*
             * Calculate joint deltas
             */
            memoryx::KBM::Vector deltas = calculateJointDeltas(kbm,
                                                               targetPosition,
                                                               predictedPosition,
                                                               simulatedProprioception,
                                                               stepSizeFactor,
                                                               maxStepSize);
 
            /*
             * Calculate new joint values
             */
            simulatedProprioception += deltas;
 
            /*
             * Predict new position
             */
            predictedPosition = kbm.predict(simulatedProprioception);
            ARMARX_IMPORTANT_S << "predicted position: " << predictedPosition;
 
            /*
             * Check if the predicted position is a solution
             */
            float distance = (targetPosition - predictedPosition).norm();
            ARMARX_IMPORTANT_S << "distance to target: " << distance;
 
            if (distance <= positionTolerance)
            {
                solution = simulatedProprioception;
                return true;
            }
 
            /*
             * Stop calculation if result doesn't get better in each iteration
             */
            if (requireImprovment && (distance >= bestDistance))
            {
                return false;
            }
 
            /*
             * If the new solution is better than the best known solution,
             * set it as the new solution!
             */
            if (distance < bestDistance)
            {
                solution = simulatedProprioception;
                bestDistance = distance;
            }
 
            /*
             * Stop calculation if deltas are small
             */
            if (deltas.norm() < minimumDelta)
            {
                return false;
            }
        }
 
        return false;
    }
 
    Ice::StringSeq
    KBMComponent::kbmNames(const Ice::Current& c)
    {
        std::vector<std::string> kbmKeys;
 
        for (auto it : kbms)
        {
            kbmKeys.push_back(it.first);
        }
 
        return kbmKeys;
    }
 
    void
    KBMComponent::clearAccumulators(const Ice::Current& c)
    {
        positionAccumulator.clear();
        proprioceptionAccumulator.clear();
        rawJointValuesAccumulator.clear();
    }
 
    void
    KBMComponent::storeToMemory(const std::string& kbmName,
                                const std::string& nodeSetName,
                                const std::string& referenceFrameName,
                                const std::string& robotName,
                                const Ice::Current& c)
    {
        memoryx::KBM::Models::KBM_ptr kbm = kbms[kbmName];
        if (kbm)
        {
            // get KBM segment from longterm memory
            auto kbmSeg = this->longtermMemoryPrx->getKBMSegment();
 
            if (kbmSeg)
            {
                memoryx::KBMDataPtr kbmData = memoryx::KBMDataPtr(
                    new memoryx::KBMData(new armarx::MatrixVariant(kbm->getControlNet()),
                                         nodeSetName,
                                         referenceFrameName,
                                         robotName));
                kbmSeg->addEntity(kbmData);
            }
        }
        else
        {
            ARMARX_WARNING << "Could not store KBM! No KBM segment in longterm memory!";
        }
 
        return;
    }
 
    void
    KBMComponent::restoreFromMemory(const std::string& kbmName,
                                    const std::string& nodeSetName,
                                    const std::string& referenceFrameName,
                                    const std::string& robotName,
                                    const Ice::Current& c)
    {
 
        auto kbmSeg = this->longtermMemoryPrx->getKBMSegment();
        std::string kbmEntityName = robotName + "_" + referenceFrameName + "_" + nodeSetName;
 
        if (kbmSeg && kbmSeg->hasEntityByName(kbmEntityName))
        {
            auto entity = kbmSeg->getEntityByName(kbmEntityName);
            memoryx::KBMDataPtr kbmData = memoryx::KBMDataPtr::dynamicCast(entity);
 
            if (kbmData)
            {
                VirtualRobot::RobotPtr robot =
                    armarx::RemoteRobot::createLocalClone(this->robotStateComponentPrx);
                int nDoF = robot->getRobotNodeSet(nodeSetName)->getSize();
 
                armarx::MatrixVariantPtr controlNet =
                    armarx::MatrixVariantPtr::dynamicCast(kbmData->getControlNet());
 
                kbms[kbmName] = memoryx::KBM::Models::KBM::createKBM(
                    nDoF,
                    controlNet->rows,
                    Eigen::VectorXd::Zero(nDoF),
                    Eigen::VectorXd::Constant(nDoF, M_PI / 4.0f),
                    controlNet->toEigen().cast<double>());
                createdFromVirtualRobot[kbmName] =
                    false; //hacky, because also KBMs created from VirtualRobot could be stored to the longterm memory
            }
            else
            {
                ARMARX_WARNING << "Could not restore KBM entity specified by: " << kbmEntityName;
            }
        }
 
        return;
    }
 
    bool
    KBMComponent::isInMemory(const std::string& nodeSetName,
                             const std::string& referenceFrameName,
                             const std::string& robotName,
                             const Ice::Current& c)
    {
        std::string kbmEntityName = robotName + "_" + referenceFrameName + "_" + nodeSetName;
 
        return (this->longtermMemoryPrx->getKBMSegment() &&
                this->longtermMemoryPrx->getKBMSegment()->hasEntityByName(kbmEntityName));
    }
} // namespace armarx