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 <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/system/FactoryCollectionBase.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
#include <ArmarXCore/util/variants/eigen3/Eigen3VariantObjectFactories.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
 
#include "AbstractMotionModel.h"
#include <MemoryX/interface/components/LongtermMemoryInterface.h>
#include <MemoryX/libraries/helpers/KinematicBezierMaps/kbm.h>
#include <MemoryX/libraries/memorytypes/entity/KBMData.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>;
} // namespace memoryx