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 <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/variant/get.hpp>
#include <boost/variant/variant.hpp>
 
#include <RobotComponents/interface/components/DMPComponentBase.h>
 
#include <MemoryX/interface/components/LongtermMemoryInterface.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/libraries/memorytypes/entity/DMPEntity.h>
 
 
#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>;
} // namespace armarx