MotionModelBodySchema.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 "MotionModelBodySchema.h"
 
#include <boost/foreach.hpp>
 
#include <MemoryX/core/entity/ProbabilityMeasures.h>
#include <MemoryX/interface/components/LongtermMemoryInterface.h>
 
namespace memoryx
{
    MotionModelBodySchema::MotionModelBodySchema(
        armarx::RobotStateComponentInterfacePrx robotStateProxy,
        std::string handNodeName,
        LongtermMemoryInterfacePrx longtermMemory) :
        AbstractMotionModel(robotStateProxy)
    {
        armarx::SharedRobotInterfacePrx currentRobotSnapshot =
            robotStateProxy->getRobotSnapshot("MotionModelBodySchemaPrediction");
        armarx::SharedRobotNodeInterfacePrx handNode =
            currentRobotSnapshot->getRobotNode(handNodeName);
 
        this->longtermMemory = longtermMemory;
        // obtain the kinematic chain
 
        //TODO: how to get the desired frame?? probably only possible in SetPoseAtLastLocalisation()?
        std::string parentName = handNode->getParent();
 
        while (!parentName.empty())
        {
            armarx::SharedRobotNodeInterfacePrx node =
                currentRobotSnapshot->getRobotNode(parentName);
 
            if (node->getType() == armarx::eRevolute)
            {
                this->parentNodeNames.push_back(parentName);
            }
 
            parentName = node->getParent();
        }
 
        this->model.reset(new KBM(parentNodeNames.size(), 16, 3.14 / 4.0));
 
        // Load from longterm memory
    }
 
    MotionModelBodySchema::~MotionModelBodySchema()
    {
        // Store to Longtermmemory
    }
 
    armarx::LinkedPosePtr
    MotionModelBodySchema::GetPredictedPoseInternal()
    {
        armarx::PosePtr oldHandNodePose = armarx::PosePtr::dynamicCast(
            poseAtLastLocalization->referenceRobot->getRobotNode(handNodeName)->getGlobalPose());
        armarx::PosePtr oldGlobalPoseOfFrame = armarx::PosePtr::dynamicCast(
            poseAtLastLocalization->referenceRobot->getRobotNode(poseAtLastLocalization->frame)
                ->getGlobalPose());
 
        armarx::SharedRobotInterfacePrx currentRobotSnapshot =
            robotStateProxy->getRobotSnapshot("MotionModelBodySchemaPrediction");
 
        // set the proprioception vector
        Eigen::VectorXf proprioception(this->parentNodeNames.size());
        unsigned int index = 0;
        BOOST_FOREACH (std::string nodeName, this->parentNodeNames)
        {
            proprioception(index) = currentRobotSnapshot->getRobotNode(nodeName)->getJointValue();
            index++;
        }
 
        Eigen::VectorXf predictionXf = this->model->predict(proprioception);
        Eigen::Matrix4f prediction = Eigen::Matrix4f::Map(predictionXf.data());
 
        armarx::LinkedPosePtr ret = new armarx::LinkedPose(
            prediction, currentRobotSnapshot->getRootNode()->getName(), currentRobotSnapshot);
        return ret;
    }
 
    void
    MotionModelBodySchema::SetPoseAtLastLocalisation(
        const armarx::LinkedPoseBasePtr& poseAtLastLocalization,
        const armarx::PoseBasePtr& globalRobotPoseAtLastLocalization,
        const Ice::Current& c)
    {
        boost::mutex::scoped_lock lock(motionPredictionLock);
 
        // get the shape|extereoception|input vector
        this->poseAtLastLocalization = poseAtLastLocalization;
        this->globalRobotPoseAtLastLocalization = globalRobotPoseAtLastLocalization;
 
        Eigen::Matrix4f mat =
            armarx::LinkedPosePtr::dynamicCast(this->poseAtLastLocalization)->toEigen();
 
        // Transform matrix to column vector
        Eigen::VectorXf shape = Eigen::VectorXf::Map(mat.data(), mat.rows() * mat.cols());
 
        armarx::SharedRobotNodeInterfacePrx handNode =
            poseAtLastLocalization->referenceRobot->getRobotNode(handNodeName);
 
        // set the proprioception vector
        Eigen::VectorXf proprioception(this->parentNodeNames.size());
        unsigned int index = 0;
        BOOST_FOREACH (std::string nodeName, this->parentNodeNames)
        {
            proprioception(index) =
                poseAtLastLocalization->referenceRobot->getRobotNode(nodeName)->getJointValue();
            index++;
        }
 
        this->model->online(proprioception, shape);
    }
 
    memoryx::MultivariateNormalDistributionBasePtr
    MotionModelBodySchema::getUncertaintyInternal()
    {
        if (uncertaintyAtLastLocalization)
        {
            return uncertaintyAtLastLocalization;
        }
        else
        {
            return NULL;
        }
    }
 
} // namespace memoryx