DMPComponent.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2015-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    RobotAPI::ArmarXObjects::DMPComponent
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
 
#include <RobotComponents/interface/components/DMPComponentBase.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/components/LongtermMemoryInterface.h>
#include <MemoryX/libraries/memorytypes/entity/DMPEntity.h>
 
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
 
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
 
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
 
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
 
#include <boost/variant/variant.hpp>
#include <boost/variant/get.hpp>
 
 
#pragma GCC diagnostic ignored "-Wunknown-pragmas"
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
#include <dmp/representation/dmp/dmpregistration.h>
#include <dmp/representation/dmpfactory.h>
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
 
#include <filesystem>
 
 
#include <ArmarXCore/core/Component.h>
//#include "dmp/representation/dmp/dmpinterface.h"
//#include "dmp/representation/dmp/basicdmp.h"
//#include "dmp/representation/dmp/dmp3rdorder.h"
//#include "dmp/representation/dmp/quaterniondmp.h"
//#include "dmp/representation/dmp/endvelodmp.h"
//#include "dmp/representation/dmp/dmp3rdorderforcefield.h"
//#include "dmp/representation/dmp/forcefielddmp.h"
//#include "dmp/representation/dmp/adaptive3rdorderdmp.h"
//#include "dmp/representation/dmp/simpleendvelodmp.h"
////#include "dmp/representation/dmp/endveloforcefielddmp.h"
////#include "dmp/representation/dmp/endveloforcefieldwithobjrepulsiondmp.h"
//#include "dmp/representation/dmp/periodictransientdmp.h"
 
#include "DMPInstance.h"
 
namespace armarx
{
    /**
     * @class DMPComponentPropertyDefinitions
     * @brief
     */
 
 
    class DMPComponentPropertyDefinitions:
        public armarx::ComponentPropertyDefinitions
    {
    public:
        DMPComponentPropertyDefinitions(std::string prefix):
            armarx::ComponentPropertyDefinitions(prefix)
        {
            //            defineRequiredProperty<std::string>("LongtermMemoryName", "Description");
            defineOptionalProperty<std::string>("LongtermMemoryName", "LongtermMemory", "Name of the LongtermMemory component");
        }
    };
 
    /**
     * @defgroup Component-DMPComponent DMPComponent
     * @ingroup RobotComponents-Components
     * @brief A brief description
     *
     *
     * Detailed Description
     */
 
    using DMPInstancePair = std::pair<DMPInstancePtr, DMPInstanceBasePrx >;
    using DMPPair = std::pair<std::string, std::pair<DMPInstancePtr, DMPInstanceBasePrx> >;
    using DMPMap = std::map<std::string, std::pair<DMPInstancePtr, DMPInstanceBasePrx> >;
 
    /**
     * @ingroup Component-DMPComponent
     * @brief The DMPComponent class
     */
    class DMPComponent :
        virtual public Component,
        virtual public DMPComponentBase
    {
    public:
 
        DMPComponent():
            ctime(0.0),
            timestep(0.001)
        {
 
        }
 
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string getDefaultName() const override
        {
            return "DMPComponent";
        }
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
        {
            return armarx::PropertyDefinitionsPtr
            {
                new DMPComponentPropertyDefinitions{getConfigIdentifier()}
            };
        }
 
        DMPInstanceBasePrx getDMP(const std::string& dmpName, const Ice::Current& = Ice::emptyCurrent) override;
 
        // DMP Database related
        DMPInstanceBasePrx getDMPFromDatabase(const std::string& dmpEntityName, const std::string& dmpName = "UNKNOWN", const Ice::Current& = Ice::emptyCurrent) override;
        DMPInstanceBasePrx getDMPFromDatabaseById(const std::string& dbId, const Ice::Current&) override;
        DMPInstanceBasePrx getDMPFromFile(const std::string& fileName, const std::string& dmpName = "UNKNOWN", const Ice::Current& = Ice::emptyCurrent) override;
 
        void storeDMPInFile(const std::string& fileName, const std::string& dmpName, const Ice::Current&) override;
        void storeDMPInDatabase(const std::string& dmpName, const std::string& name, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeDMPFromDatabase(const std::string& name, const ::Ice::Current& = Ice::emptyCurrent) override;
        void removeDMPFromDatabaseById(const std::string& dbId, const Ice::Current&) override;
 
 
        //transmit data from client to server (using ice)
        DMPInstanceBasePrx instantiateDMP(const std::string& dmpName, const std::string& DMPType, int kernelSize, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void setDMPState(const std::string& dmpName, const ::armarx::cStateVec& state, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setGoal(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current& = Ice::emptyCurrent) override;
        void setStartPosition(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current& = Ice::emptyCurrent) override;
        void setCanonicalValues(const std::string& dmpName, const Ice::DoubleSeq& value, const Ice::Current& = Ice::emptyCurrent) override;
 
        void readTrajectoryFromFile(const std::string& dmpName, const std::string& file, double times = 1, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void trainDMP(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setAmpl(const std::string& dmpName, int dim, double value, const ::Ice::Current& = Ice::emptyCurrent) override;
        double getAmpl(const std::string& dmpName, int dim, const ::Ice::Current& = Ice::emptyCurrent) override;
 
 
        double getTemporalFactor(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
        void setTemporalFactor(const std::string& dmpName, double tau, const ::Ice::Current& = Ice::emptyCurrent) override;
 
 
        Vec2D calcTrajectory(const std::string& dmpName, double startTime, double timeStep, double endTime,
                             const ::Ice::DoubleSeq& goal,
                             const cStateVec& states,
                             const ::Ice::DoubleSeq& canonicalValues, double temporalFactor, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        // time manager
        double getTimeStep(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            return timestep;
        }
 
        void setTimeStep(double ts, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        double getCurrentTime(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void resetTime(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void resetCanonicalValues(const ::Ice::Current& = Ice::emptyCurrent) override;
 
 
        double getDampingFactor(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        double getSpringFactor(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        double getForceTerm(const std::string& dmpName, const Ice::DoubleSeq& canonicalValues, int dim, const Ice::Current& = Ice::emptyCurrent) override;
 
        //transmit data from server to client (using ice)
        cStateVec getNextState(const std::string& dmpName, const cStateVec& states, const ::Ice::Current& = Ice::emptyCurrent) override;
        cStateVec getCurrentState(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
        Ice::DoubleSeq getCanonicalValues(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
        Ice::DoubleSeq getTrajGoal(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
        ::armarx::cStateVec getTrajStartState(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        std::string getDMPType(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        Ice::DoubleSeq getStartPosition(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        SVector getDMPNameList(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void eraseDMP(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
        bool isDMPExist(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
        int getDMPDim(const std::string& dmpName, const ::Ice::Current& = Ice::emptyCurrent) override;
 
 
        double ctime;
        double timestep;
        std::vector<int> usedDimensions;
 
    private:
        DMPInstanceBasePrx findInstancePrx(std::string name);
        DMPInstancePtr findInstancePtr(std::string name);
        DMPInstanceBasePrx createDMPInstancePrx(DMPInstancePtr dmpPtr);
 
    protected:
        DMPMap dmpPool;
 
        memoryx::LongtermMemoryInterfacePrx longtermMemoryPrx;
        memoryx::PersistentDMPDataSegmentBasePrx dmpDataMemoryPrx;
 
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void onDisconnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void onExitComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        //        virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions();
    };
    using DMPComponentPtr = ::IceInternal::Handle< ::armarx::DMPComponent>;
}