MotionModelKBM.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "MotionModelKBM.h"
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <MemoryX/core/entity/ProbabilityMeasures.h>
 
#include <VirtualRobot/LinkedCoordinate.h>
 
#include <MemoryX/libraries/memorytypes/entity/KBMData.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Robot.h>
 
using namespace VirtualRobot;
 
namespace memoryx
{
    MotionModelKBM::MotionModelKBM(std::string referenceNodeName, std::string nodeSetName,
                                   armarx::RobotStateComponentInterfacePrx robotStateProxy, memoryx::LongtermMemoryInterfacePrx longtermMemory): AbstractMotionModel(robotStateProxy)
    {
        this->nodeSetName = nodeSetName;
        this->referenceNodeName = referenceNodeName;
 
        this->longtermMemory = longtermMemory;
        memoryName = robotStateProxy->getRobotName() + "_" + referenceNodeName  + "_" + nodeSetName;
        ARMARX_IMPORTANT_S << VAROUT(memoryName);
        init();
    }
 
    void MotionModelKBM::setPoseAtLastLocalisation(const armarx::LinkedPoseBasePtr& poseAtLastLocalization, const armarx::PoseBasePtr& globalRobotPoseAtLastLocalization,
            const MultivariateNormalDistributionBasePtr& uncertaintyAtLastLocalization, const Ice::Current& c)
    {
        std::unique_lock lock(motionPredictionLock);
        this->poseAtLastLocalization = poseAtLastLocalization;
        timeOfLastSuccessfulLocalization = armarx::TimestampVariantPtr::dynamicCast(timeOfLastLocalizationStart->clone());
 
        if (globalRobotPoseAtLastLocalization)
        {
            this->globalRobotPoseAtLastLocalization = globalRobotPoseAtLastLocalization;
        }
 
        if (uncertaintyAtLastLocalization)
        {
            this->uncertaintyAtLastLocalization = uncertaintyAtLastLocalization;
        }
 
        armarx::LinkedPosePtr pose = armarx::LinkedPosePtr::dynamicCast(poseAtLastLocalization);
 
        pose->changeFrame(referenceNodeName);
        auto poseKBM = armarx::LinkedPosePtr::dynamicCast(pose->clone());
        poseKBM->changeFrame(robot->getRobotNodeSet(nodeSetName)->getKinematicRoot()->getName());
 
        KBM::Matrix shape = poseKBM->toEigen().block<3, 1>(0, 3).cast<double>();
        //        shape.conservativeResize(3, 1);
 
        kbm->online(this->getJointAngles(poseAtLastLocalization->referenceRobot).cast<double>(), shape);
 
    }
 
    armarx::LinkedPosePtr MotionModelKBM::getPredictedPoseInternal()
    {
        std::unique_lock lock(motionPredictionLock);
        armarx::LinkedPosePtr linkedPose = armarx::LinkedPosePtr::dynamicCast(poseAtLastLocalization);
 
        //auto snapshot = robotStateProxy->getRobotSnapshot(robotStateProxy->getSynchronizedRobot()->getName());
 
        auto snapShot = robotStateProxy->getRobotSnapshot(robot->getName());
        Eigen::MatrixXd jointAngles = this->getJointAngles(snapShot).cast<double>()/*.colwise().reverse().reverse()*/;
        ARMARX_DEBUG_S << VAROUT(jointAngles);
        auto time = IceUtil::Time::now();
        Eigen::Vector3f kbmPos = this->kbm->predict(jointAngles).cast<float>();
        ARMARX_DEBUG_S << "KBM prediction Took: " << (IceUtil::Time::now() - time).toMilliSecondsDouble() << " ms";
        auto nodeSet = robot->getRobotNodeSet(nodeSetName);
        Eigen::Matrix4f pose = nodeSet->getTCP()->getGlobalPose();
        armarx::FramedPosition framedKbmPos(kbmPos, nodeSet->getKinematicRoot()->getName(), linkedPose->agent);
        framedKbmPos.changeToGlobal(robot);
        ARMARX_DEBUG_S << pose.block<3, 1>(0, 3) << " vs. " << framedKbmPos.toEigen();
        ARMARX_DEBUG_S << "Difference between KBM and robot model: " << (pose.block<3, 1>(0, 3) - framedKbmPos.toEigen()).norm() << " mm";
        pose.block<3, 1>(0, 3) = framedKbmPos.toEigen();
        //shape.conservativeResize(4, 4);
        //        FramedPose framedPose(pose, nodeSet->getKinematicRoot()->getName(), linkedPose->agent);
        armarx::LinkedPosePtr ret = new armarx::LinkedPose(pose, armarx::GlobalFrame, snapShot);
        return ret;
 
    }
 
    Eigen::MatrixXf MotionModelKBM::getJointAngles(armarx::SharedRobotInterfacePrx robotPrx)
    {
        armarx::RemoteRobot::synchronizeLocalClone(robot, robotPrx);
        Eigen::MatrixXf proprioception(nDoF, 1);
        std::vector<float> jointAngles = robot->getRobotNodeSet(nodeSetName)->getJointValues();
        ARMARX_CHECK_EXPRESSION(nDoF == (int)jointAngles.size());
        for (int i = 0; i < nDoF; i++)
        {
            proprioception(i, 0) = jointAngles[i];
        }
 
        return proprioception;
    }
 
    void MotionModelKBM::periodicUpdate()
    {
        ARMARX_INFO_S << "KBM Database update";
 
 
        if (longtermMemory)
        {
            auto kbmSeg = longtermMemory->getKBMSegment();
            std::unique_lock lock(motionPredictionLock);
 
 
 
            if (kbmSeg)
            {
                if (kbmSeg->hasEntityByName(memoryName))
                {
                    auto entity = kbmSeg->getEntityByName(memoryName);
                    KBMDataPtr kbmData = KBMDataPtr::dynamicCast(entity);
 
                    if (kbmData)
                    {
                        if (kbm)
                        {
                            kbmData->setControlNet(new armarx::MatrixVariant(kbm->getControlNet()));
                            kbmSeg->updateEntity(kbmData->getId(), kbmData);
                        }
                        else
                        {
                            ARMARX_WARNING_S << "No KBM!";
                        }
                    }
                    else
                    {
                        ARMARX_WARNING_S << "Entity is not KBMData. MemoryName:  " << memoryName << " Entity ice_id:" << entity->ice_id();
                    }
 
                }
                else
                {
                    if (kbm)
                    {
                        KBMDataPtr kbmData = KBMDataPtr(new KBMData(new armarx::MatrixVariant(kbm->getControlNet()), nodeSetName, referenceNodeName, robotStateProxy->getRobotName()));
                        ARMARX_INFO_S << "Adding new kbm with name " << kbmData->getName();
                        kbmSeg->addEntity(kbmData);
                    }
                    else
                    {
                        ARMARX_WARNING_S << "No KBM!";
                    }
 
                }
            }
            else
            {
                ARMARX_WARNING_S << "No KBM Segment!";
            }
        }
    }
 
    void MotionModelKBM::init()
    {
        ARMARX_DEBUG << "init";
        //armarx::SharedRobotInterfacePrx currentRobotSnapshot = ;//getRobotSnapshot("KBMSnapshot");
        robot = armarx::RemoteRobot::createLocalCloneFromFile(this->robotStateProxy);
 
        nDoF = robot->getRobotNodeSet(this->nodeSetName)->getSize();
 
        auto kbmSeg = longtermMemory->getKBMSegment();
        std::unique_lock lock(motionPredictionLock);
 
        ARMARX_DEBUG << VAROUT(memoryName);
        if (kbmSeg && kbmSeg->hasEntityByName(memoryName))
        {
            ARMARX_VERBOSE << "Fetching KBM from memory";
            auto entity = kbmSeg->getEntityByName(memoryName);
            KBMDataPtr kbmData = KBMDataPtr::dynamicCast(entity);
 
            if (kbmData)
            {
 
                armarx::MatrixVariantPtr controlNet = armarx::MatrixVariantPtr::dynamicCast(kbmData->getControlNet());
 
                this->kbm = KBM::Models::KBM::createKBM(nDoF, controlNet->rows, Eigen::VectorXd::Zero(nDoF), Eigen::VectorXd::Constant(nDoF, M_PI / 4.0f), controlNet->toEigen().cast<double>());
            }
            else
            {
                ARMARX_WARNING_S << "Entity is not KBMData. MemoryName:  " << memoryName << " Entity ice_id:" << entity->ice_id();
            }
 
            // get file from long-term memory and store in name in
            //std::string tempFileName;
            //this->kbm.reset(new KBM::Models::KBM(nDoF,16,M_PI/4.0f));
            //this->kbm->restore(tempFileName);
        }
        else
        {
            // If longtermmemory = empty
            ARMARX_VERBOSE << "Longterm memory empty";
 
            try
            {
 
                auto nodeSet = robot->getRobotNodeSet(nodeSetName);
                this->kbm = CreateKBMFromSamples(robot,
                                                 nodeSet,
                                                 nodeSet->getKinematicRoot());
                KBMDataPtr kbmData = new KBMData(new armarx::MatrixDouble(kbm->getControlNet()), nodeSetName,
                                                 referenceNodeName,
                                                 robot->getName());
                kbmSeg->addEntity(kbmData);
                ARMARX_VERBOSE << "Adding new entity with name " << kbmData->getName();
                //                this->kbm = KBM::Models::KBM::createFromVirtualRobot(chain, FoR, Eigen::VectorXd::Constant(nDoF, M_PI / 4.0), false);
            }
            catch (VirtualRobot::VirtualRobotException& e)
            {
                ARMARX_ERROR_S << "KBM creation exception. " << e.what();
 
                /*
                 * Probably an invalid Kinematic chain.
                 * Throw an adequate exception here!
                 */
            }
        }
 
 
 
        ARMARX_CHECK_EXPRESSION(kbm);
 
        if (longtermMemory)
        {
            updaterThreadTask = new armarx::PeriodicTask<MotionModelKBM>(this, &MotionModelKBM::periodicUpdate, 1000 * 60);
            updaterThreadTask->start();
        }
    }
 
    KBM::Models::KBM_ptr MotionModelKBM::CreateKBMFromSamples(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr nodeSet, VirtualRobot::SceneObjectPtr referenceFrame)
    {
        ARMARX_INFO_S << "Starting to generate KBM with random samples for nodeset " <<  nodeSet->getName();
        nodeSet->print();
        KBM::Models::KBM_ptr kbm;
        //        KBMComponentInterfacePrx kbmComponent = getKbmComponent();
        std::vector<RobotNodePtr> nodes = nodeSet->getAllRobotNodes();
        RobotNodePtr root = nodeSet->getKinematicRoot();
        RobotNodePtr TCP = nodeSet->getTCP();
        size_t nJoints = nodes.size();
        size_t nOutputDim = 3;
        Eigen::VectorXf jointMin = Eigen::VectorXf::Zero(nJoints, 1);
        Eigen::VectorXf jointMax = Eigen::VectorXf::Zero(nJoints, 1);
 
        for (size_t i = 0; i < nJoints; i++)
        {
            RobotNodePtr n = nodes[i];
            jointMax(i) = n->getJointLimitHi() - 0.1f;
            jointMin(i) = n->getJointLimitLo() + 0.1f;
        }
 
        //        if(kbmComponent->isInMemory(in.getnodeSetName(),root->getName(),robot->getName()))
        //        {
        //            ARMARX_INFO << "Loading KBM from memory";
        //            kbmComponent->restoreFromMemory(in.getkbmName(),in.getnodeSetName(),root->getName(), robot->getName());
        //        }
        //        else
        //        {
        //            ARMARX_INFO << "KBM not found in memory. Batch training KBM.";
 
        size_t nTrainingSamples = (size_t)pow(nOutputDim, nJoints);
        kbm.reset(new memoryx::KBM::Models::KBM(nJoints, nOutputDim, M_PI / 4.0f));
        std::vector<memoryx::KBM::Real> proprioceptionAccumulator;
        std::vector<memoryx::KBM::Real> positionAccumulator;
        ARMARX_INFO_S << VAROUT(nTrainingSamples);
        for (size_t i = 0; i < nTrainingSamples; i++)
        {
            Ice::DoubleSeq proprioception, pos;
 
            CreateSample(nodeSet, jointMax, jointMin, root, TCP, proprioception, pos);
 
            proprioceptionAccumulator.insert(proprioceptionAccumulator.end(), proprioception.begin(), proprioception.end());
            positionAccumulator.insert(positionAccumulator.end(), pos.begin(), pos.end());
        }
        auto mapVectorToMatrix = [](const std::vector<memoryx::KBM::Real>& vec, size_t rows)
        {
            size_t cols = vec.size() / rows;
            return Eigen::Map<const memoryx::KBM::Matrix>(vec.data(), rows, cols);
        };
 
        kbm->batch(mapVectorToMatrix(proprioceptionAccumulator, nJoints),
                   mapVectorToMatrix(positionAccumulator, nOutputDim), KBM::Models::KBM::STANDARD, 0.0f);
        //           kbmComponent->createKBM(in.getkbmName(), nJoints, 3, M_PI/4.0f);
        //           kbmComponent->batchAccumulator(in.getkbmName(),0,0.0f);
        //           kbmComponent->storeToMemory(in.getkbmName(),in.getnodeSetName(),root->getName(), robot->getName());
 
        //           kbmComponent->printAccumulatorStatistics(3, nJoints);
        //           kbmComponent->printErrorsAccumulator(in.getkbmName());
        //           kbmComponent->clearAccumulators();
        //        }
 
 
        //        unsigned int nEvaluationSamples = (int)pow(nOutputDim, nJoints) / 20;
        //        proprioceptionAccumulator.clear();
        //        positionAccumulator.clear();
        //        for (size_t i = 0; i < nEvaluationSamples; i++)
        //        {
        //            Ice::DoubleSeq prop, pos;
 
        //            CreateSample(nodeSet, jointMax, jointMin, root, TCP, prop, pos);
        //            proprioceptionAccumulator.insert(proprioceptionAccumulator.end(), prop.begin(), prop.end());
        //            positionAccumulator.insert(positionAccumulator.end(), pos.begin(), pos.end());
        //            ARMARX_INFO_S << "Error: " << (kbm->predict(mapVectorToMatrix(prop, nJoints)) - mapVectorToMatrix(pos, nOutputDim)).norm() << " mm";
        //        }
        //        auto errors = kbm->getErrors(mapVectorToMatrix(proprioceptionAccumulator, nJoints),
        //                                     mapVectorToMatrix(positionAccumulator, nOutputDim));
        //        ARMARX_INFO_S << "Errors: " << errors;
        ARMARX_INFO_S << "DONE";
 
        return kbm;
 
 
    }
 
 
 
    memoryx::MultivariateNormalDistributionBasePtr MotionModelKBM::getUncertaintyInternal()
    {
        if (uncertaintyAtLastLocalization)
        {
            auto handNodeName = robot->getRobotNodeSet(nodeSetName)->getTCP()->getName();
            armarx::PosePtr oldHandNodePose = armarx::PosePtr::dynamicCast(poseAtLastLocalization->referenceRobot->getRobotNode(handNodeName)->getPoseInRootFrame());
            armarx::PosePtr newHandNodePose = armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()->getRobotNode(handNodeName)->getPoseInRootFrame());
 
            // additional uncertainty is 0.07 * the distance that the hand moved since the last localization
            float dist = (oldHandNodePose->toEigen().block<3, 1>(0, 3) - newHandNodePose->toEigen().block<3, 1>(0, 3)).norm();
            dist *= 0.3;
            Eigen::Matrix3f additionalUncertainty = dist * dist * Eigen::Matrix3f::Identity();
 
            Eigen::Matrix3f oldUncertainty = MultivariateNormalDistributionPtr::dynamicCast(uncertaintyAtLastLocalization)->toEigenCovariance();
            Eigen::Matrix3f newUncertainty = oldUncertainty + additionalUncertainty;
 
            return new MultivariateNormalDistribution(armarx::Vector3Ptr::dynamicCast(poseAtLastLocalization->position)->toEigen(), newUncertainty);
        }
        else
        {
            return NULL;
        }
    }
 
    void MotionModelKBM::ice_postUnmarshal()
    {
        init();
    }
 
    void MotionModelKBM::CreateSample(RobotNodeSetPtr nodeSet, Eigen::VectorXf jointMax, Eigen::VectorXf jointMin, RobotNodePtr root, RobotNodePtr TCP, Ice::DoubleSeq& prop, Ice::DoubleSeq& shape)
    {
        unsigned int nJoints = nodeSet->getSize();
        Eigen::VectorXf rnd = Eigen::VectorXf::Random(nJoints, 1);
        Eigen::VectorXf sample = (rnd + Eigen::VectorXf::Ones(nJoints, 1)).cwiseProduct((jointMax - jointMin) / 2) + jointMin;
 
        for (size_t k = 0; k < nJoints; k++)
        {
            nodeSet->getNode(k)->setJointValue(sample(k));
        }
        std::vector<float> actualJointValues = nodeSet->getJointValues();
        //        for (size_t l = 0; l < nJoints; l++)
        //        {
        //            actualJointValues.push_back(nodeSet->getNode(l)->getJointValue());
        //        }
 
        Eigen::Vector3f tcpPos = TCP->getTransformationFrom(root).block<3, 1>(0, 3);
 
        for (size_t j = 0; j < nJoints; j++)
        {
            prop.push_back(actualJointValues[j]);
        }
 
        assert(tcpPos.rows() >= 0);
        for (size_t k = 0; k < static_cast<std::size_t>(tcpPos.rows()); k++)
        {
            shape.push_back(tcpPos(k));
        }
    }
 
 
}