MotionModelKBM.h

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* 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::MotionModelKBM
* @author     David Schiebener ( Schiebener at kit dot edu)
* @date       2013
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
 
#include <MemoryX/libraries/helpers/KinematicBezierMaps/kbm.h>
#include <MemoryX/libraries/memorytypes/entity/KBMData.h>
#include "AbstractMotionModel.h"
#include <ArmarXCore/observers/variant/ChannelRef.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
 
#include <MemoryX/interface/components/LongtermMemoryInterface.h>
#include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
#include <VirtualRobot/VirtualRobot.h>
#include<ArmarXCore/core/services/tasks/PeriodicTask.h>
namespace memoryx
{
    class MotionModelKBM:
        virtual public AbstractMotionModel,
        virtual public MotionModelKBMBase
    {
    public:
        MotionModelKBM(std::string referenceNodeName, std::string nodeSetName,
                       armarx::RobotStateComponentInterfacePrx robotStateProxy, memoryx::LongtermMemoryInterfacePrx longtermMemoryPrx);
        AbstractMotionModel::EMotionModelType getMotionModelType() override
        {
            return AbstractMotionModel::eMotionModelKBM;
        }
 
        static void CreateSample(VirtualRobot::RobotNodeSetPtr nodeSet, Eigen::VectorXf jointMax, Eigen::VectorXf jointMin, VirtualRobot::RobotNodePtr root, VirtualRobot::RobotNodePtr TCP, Ice::DoubleSeq& prop, Ice::DoubleSeq& shape);
        static KBM::Models::KBM_ptr CreateKBMFromSamples(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr nodeSet, VirtualRobot::SceneObjectPtr referenceFrame);
        void setPoseAtLastLocalisation(const armarx::LinkedPoseBasePtr& poseAtLastLocalization, const armarx::PoseBasePtr& globalRobotPoseAtLastLocalization,
                                       const MultivariateNormalDistributionBasePtr& uncertaintyAtLastLocalization, const Ice::Current& c = Ice::emptyCurrent) override;
    protected:
 
        armarx::LinkedPosePtr getPredictedPoseInternal() override;
        MultivariateNormalDistributionBasePtr getUncertaintyInternal() override;
 
        void ice_postUnmarshal() override;
 
 
 
        Eigen::MatrixXf getJointAngles(armarx::SharedRobotInterfacePrx robotPrx);
 
        void periodicUpdate();
        // for the object factory
        template <class IceBaseClass, class DerivedClass>
        friend class armarx::GenericFactory;
        MotionModelKBM() { }
        KBM::Models::KBM_ptr kbm; // NICHT umziehen nach .ice
        VirtualRobot::RobotPtr robot; // NICHT umziehen nach .ice
        armarx::PeriodicTask<MotionModelKBM>::pointer_type updaterThreadTask; // NICHT umziehen nach .ice
        std::string memoryName;
    private:
        void init();
 
 
 
 
 
    };
    using MotionModelKBMPtr = IceInternal::Handle<MotionModelKBM>;
}