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 <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotComponents/interface/components/DMPComponentBase.h>
 
#include <MMM/MMMCore.h>
#include <dmp/representation/dmp/dmpregistration.h>
 
//#include <dmp/representation/dmp/dmpregistration.h>
#include "dmp/representation/dmp/adaptive3rdorderdmp.h"
#include "dmp/representation/dmp/basicdmp.h"
#include "dmp/representation/dmp/dmp3rdorder.h"
#include "dmp/representation/dmp/dmp3rdorderforcefield.h"
#include "dmp/representation/dmp/dmpinterface.h"
#include "dmp/representation/dmp/endvelodmp.h"
#include "dmp/representation/dmp/endveloforcefielddmp.h"
#include "dmp/representation/dmp/endveloforcefieldwithobjrepulsiondmp.h"
#include "dmp/representation/dmp/forcefielddmp.h"
#include "dmp/representation/dmp/periodicdmp.h"
#include "dmp/representation/dmp/quaterniondmp.h"
#include "dmp/representation/dmp/simpleendvelodmp.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>;
 
} // namespace armarx