DMPInstance.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    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 <MMM/MMMCore.h>
 
#include <dmp/representation/dmp/dmpregistration.h>
 
#include <RobotComponents/interface/components/DMPComponentBase.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/logging/Logging.h>
 
//#include <dmp/representation/dmp/dmpregistration.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/periodicdmp.h"
#include "dmp/representation/dmp/taskspacedmp.h"
 
 
namespace armarx
{
 
#define ARMARX_DMPTYPE_BASICDMP boost::serialization::guid<DMP::BasicDMP>()
#define ARMARX_DMPTYPE_ENDVELODMP boost::serialization::guid<DMP::EndVeloDMP>()
#define ARMARX_DMPTYPE_SIMPLEENDVELODMP boost::serialization::guid<DMP::SimpleEndVeloDMP>()
#define ARMARX_DMPTYPE_FORCEFIELDDMP boost::serialization::guid<DMP::ForceFieldDMP>()
#define ARMARX_DMPTYPE_ENDVELFORCEFILELDDMP boost::serialization::guid<DMP::EndVeloForceFieldDMP>()
#define ARMARX_DMPTYPE_DMP3RDORDER boost::serialization::guid<DMP::DMP3rdOrder>()
#define ARMARX_DMPTYPE_DMP3RDORDERFORCEFIELD boost::serialization::guid<DMP::DMP3rdOrderForceField>()
#define ARMARX_DMPTYPE_ADAPTIVEGOAL3RDORDERDMP boost::serialization::guid<DMP::AdaptiveGoal3rdOrderDMP>()
#define ARMARX_DMPTYPE_QUATERNIONDMP boost::serialization::guid<DMP::QuaternionDMP>()
#define ARMARX_DMPTYPE_PERIODICDMP boost::serialization::guid<DMP::PeriodicDMP>()
#define ARMARX_DMPTYPE_TASKSPACEDMP boost::serialization::guid<DMP::TaskSpaceDMP>()
 
 
    using paraType = boost::variant<double, DMP::DVec, Eigen::Quaternionf>;
    using configMap = std::map<std::string, paraType >;
    using configPair = std::pair<std::string, paraType >;
    using TrajVec = DMP::Vec<DMP::SampledTrajectoryV2>;
 
 
 
    /**
         * @defgroup DMPInstance DMPInstance
         * @ingroup RobotComponents-Components
         * @brief The DMP Instance class
         *
         * Manages an Instance of a DMP, the DMP can be of different types. Each Instance manages exactly one DMP, the Instances are managed with the @see DMPComponent
         * The Instance manages the Configuration of the DMP and can calculate the states of the DMP and the whole Trajectory for all dimensions for different start and target positions.
         *
         */
    class DMPInstance :
        virtual public armarx::Logging,
        virtual public DMPInstanceBase
    {
 
    public:
        /**
         * @brief DMPInstance constructs an empty instance of a basicDMP
         */
        DMPInstance():
            dmpType(ARMARX_DMPTYPE_BASICDMP),
            dmp(new DMP::BasicDMP()),
            ctime(0),
            timestep(0.001)
 
        {}
 
        DMPInstance(DMP::DMPInterfacePtr dmpInter)
        {
            dmp = dmpInter;
            dmpType = dmp->getDMPType();
            ctime = 0;
            timestep = 0.001;
        }
 
        /**
         * @brief DMPInstance constructs a new instance of the specified DMP
         * @param dmpInter pointer to the dmp
         * @param DMPType type of dmp
         */
        DMPInstance(DMP::DMPInterfacePtr dmpInter, std::string DMPType);
 
        // setter functions
        /**
         * @brief setParameter set parameters for
         * @param paraId pointer to the dmp
         * @param value type of dmp
         */
        void setParameter(const std::string& paraId, const Ice::DoubleSeq& value, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief setGoal set the goal for dmp
         * @param value the vector used to specify goal vector
         */
        void setGoal(const Ice::DoubleSeq& value, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief setStartPosition set the start position for dmp
         * @param value: the vector used to define start position
         */
        void setStartPosition(const Ice::DoubleSeq& value, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief setDMPState set current state for dmp
         * @param state state to set
         */
        void setDMPState(const cStateVec& state, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief setConfigurationMap set dmp configurations using configMap
         * @param value: configMap saves all possible configurations for dmp
         */
        void setConfigurationMap(const DMPConfigMap& value, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief setCanonicalValues: set dmp canonicalValues to control the dmp's state
         * @param value: current canonical values needed
         */
        void setCanonicalValues(const Ice::DoubleSeq& value, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief setTemporalFactor: set dmp temporal factor
         */
        void setTemporalFactor(double value, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief setAmpl: set amplitude for dmp
         * @param dim: the dimension concerned
         * @param value: new amplitude
         */
        void setAmpl(int dim, double value, const ::Ice::Current& = Ice::emptyCurrent) override;
 
 
        // getter functions
        /**
         * @brief getAmpl: get current amplitude of a dimension
         * @param dim: the dimension concerned
         */
        double getAmpl(int dim, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief getForceTerm: get force termf for one dimension at a specified canonical values
         * @param canVal: corresponding canonical value
         * @param dim: the dimension concerned
         */
        double getForceTerm(const Ice::DoubleSeq& canVal, int dim, const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief getTemporalFactor: get current temporal factor
         */
        double getTemporalFactor(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief getCanonicalValues: get current canonical values
         */
        Ice::DoubleSeq getCanonicalValues(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            return canonicalValues;
        }
 
        /**
         * @brief getDampingFactor: get dmp's damping factor
         */
        double getDampingFactor(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            return dmp->getDampingFactor();
        }
 
        /**
         * @brief getSpringFactor: get dmp's spring factor
         */
        double getSpringFactor(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            return dmp->getSpringFactor();
        }
 
        /**
         * @brief getDMPType: get dmp type
         */
        std::string getDMPType(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            return dmp->getDMPType();
        }
 
        /**
         * @brief getDMPDim: get dimesions of dmp
         */
        int getDMPDim(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            return dmp->dim();
        }
 
        /**
         * @brief getStartPosition: get start position of dmp
         */
        Ice::DoubleSeq getStartPosition(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            return dmp->getStartPositions();
        }
 
        /**
         * @brief getTrajGoal: get the goal of the training trajectory
         */
        Ice::DoubleSeq getTrajGoal(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        /**
         * @brief getTrajStartState: get the start state of the training trajectory
         */
        cStateVec getTrajStartState(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        Ice::DoubleSeq getGoal(const Ice::Current& = Ice::emptyCurrent);
 
 
        // DMP trainer
        void trainDMP(const ::Ice::Current& = Ice::emptyCurrent) override;
 
        void readTrajectoryFromFile(const std::string& file, double times = 1, const ::Ice::Current& = Ice::emptyCurrent) override;
 
 
        // dmp calculation
        /**
         * @brief getNextState: get next state according to the state given
         */
        cStateVec getNextStateWithCurrentState(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            return getNextState(getCurrentState());
        }
        /**
         * @brief getNextState: get next state according to the state given
         * @param states: the current state given to the function
         */
        cStateVec getNextState(const cStateVec& states, const ::Ice::Current& = Ice::emptyCurrent) override;
        /**
         * @brief getCurrentState: get current state in dmp
         */
        cStateVec getCurrentState(const ::Ice::Current& = Ice::emptyCurrent) override
        {
            return dmpState;
        }
 
        /**
         * @brief calcTrajectory solves the DMP for all Timesteps
         *
         * Calculates the Trajectory for the trained DMP given the goal and inital State for all Dimensions
         *
         * @param timestamps The timesteps for which the trajecotry should be calculated
         * @param goal The target(goal) position for all dimensions of the trajectory
         * @param initStates the initial States of the DMP
         * @param canonicalValues The canonical Value for the DMP
         * @param temporalFactor his factor determines the duration of the
         * trajectory. The duration scales linear with this factor.
         * @return The Trajectory which is created by the DMP, containing all the States of the DMP for each timestep
         */
        Vec2D calcTrajectory(double startTime, double timestep, double endTime,
                             const ::Ice::DoubleSeq& goal, const cStateVec& initStates, const ::Ice::DoubleSeq& canonicalValues, double temporalFactor, const Ice::Current& = Ice::emptyCurrent) override;
 
 
 
 
        // local functions
        configMap createConfigMap(DMP::Vec<std::string> paraIDs, DMP::Vec<paraType> paraVals);
 
        DMP::DMPInterfacePtr getDMP()
        {
            return dmp;
        }
 
        // time manager
        void setTimeStep(double ts)
        {
            timestep = ts;
        }
 
        void setCurrentTime(double t)
        {
            ctime = t;
        }
 
        double getCurrentTime()
        {
            return ctime;
        }
        nStateValues calcNextStateFromConfig(double t, double currentT, const Vec2D& currentStates, const Ice::DoubleSeq& canonicalValues, const Ice::Current& = Ice::emptyCurrent) override;
        nStateValues calcNextState(double t, const Ice::DoubleSeq& goal, double currentT, const Vec2D& currentStates, const Ice::DoubleSeq& canonicalValues, double temporalFactor, const::Ice::Current& = Ice::emptyCurrent) override;
        Vec2D calcTrajectoryV2(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& goal, const Vec2D& initStates, const Ice::DoubleSeq& canonicalValues, double temporalFactor, const Ice::Current& = Ice::emptyCurrent) override;
        Vec2D calcTrajectoryFromConfig(const Ice::DoubleSeq& timestamps, const Vec2D& initStates, const Ice::DoubleSeq& canonicalValues, const Ice::Current& = Ice::emptyCurrent) override;
    protected:
        std::string dmpType;
        DMP::DMPInterfacePtr dmp;
        configMap configs;
        double ctime;
 
        double timestep;
 
        TrajVec trajs;
        std::string dmpName;
        DMP::DVec canonicalValues;
        cStateVec dmpState;
 
    };
 
 
    using DMPInstancePtr = IceInternal::Handle<DMPInstance>;
 
}