MP.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/>.
 *
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/BasicTypes.h>
#include <ArmarXCore/interface/serialization/Eigen.h>
 
#include <mplib/factories/VMPFactory.h>
#include <mplib/factories/AbstractMPFactory.h>
#include <mplib/factories/MPFactoryCreator.h>
#include <mplib/representation/vmp/VMP.h>
#include <mplib/representation/vmp/PrincipalComponentVMP.h>
#include <mplib/representation/vmp/TaskSpacePrincipalComponentVMP.h>
#include <SimoxUtility/json.h>
 
 
namespace armarx::control::common::mp
{
    using VMPPtr = std::shared_ptr<mplib::representation::vmp::PrincipalComponentVMP>;
    using MutexType = std::recursive_mutex;
    using LockGuardType = std::lock_guard<std::recursive_mutex>;
 
    struct MPInput  {virtual ~MPInput()  {}};
    struct MPOutput {virtual ~MPOutput() {}};
    using MPInputPtr = std::shared_ptr<MPInput>;
    using MPOutputPtr = std::shared_ptr<MPOutput>;
 
    class MP
    {
    public:
        struct ListViaPoint
        {
            double canonicalValue;
            std::vector<double> viaPointValue;
        };
 
        struct DictViaPoint
        {
            double canonicalValue;
            std::map<std::string, double> viaPointValue;
        };
 
        struct MPConfig
        {
            std::string name;
            std::string className = "MP";
            std::string mpTypeString = "PrincipalComponent";
//            mplib::representation::VMPType vmpType = mplib::representation::VMPType::PrincipalComponent;
            std::string mpMode = "MinimumJerk";
            std::string mpStyle = "Discrete";
            std::string regressionModel = "default";
            int kernelSize = 100;
            float damping = 20.0f;
            double tau = 1;
            double amplitude = 1;
            double durationSec = 10;
 
            std::vector<std::string> fileList;
            bool stopWithMP = false;
            std::vector<ListViaPoint> viaPoints;
 
            /// phase stop parameters ps = PhaseStop
            bool enablePhaseStop = false;
            double maxValue = 100;
            double slop = 1000;
            double goDist = 80;
            double backDist = 50;
            double psKpPos = 1;
            double psKdPos = 2;
            double psKpOri = 1;
            double psKdOri = 0.1;
            double psMM2Radian = 100;
 
//            void resetVMPType()
//            {
//                if (mpTypeString != "")
//                {
//                    vmpType = mplib::representation::VMPType::_from_string_nocase(mpTypeString.c_str());
//                }
//            }
        };
        struct MPListConfig
        {
            std::vector<MPConfig> mpList;
        };
 
//        MP(const std::string& name, nlohmann::json& userConfig, nlohmann::json& defaultConfig);
        MP(const MPConfig& c);
        virtual ~MP() = default;
 
 
 
        std::string getMPName();
        std::string getClassName();
 
        /// control
        void start();
        void start(const Ice::DoubleSeq& goals);
        void start(const Ice::DoubleSeq& goals, Ice::Double timeDuration);
        void start(const Ice::DoubleSeq& goals, const Ice::DoubleSeq& starts, Ice::Double timeDuration);
        void start(const Ice::DoubleSeq& goals, const Ice::DoubleSeq& starts);
        void stop();
        void pause();
        void resume();
        void reset();
        bool isFinished();
 
        /// setting
        void learnFromCSV(const Ice::StringSeq& fileNames = std::vector<std::string>());
 
        void setGoal(const Ice::DoubleSeq& goals);
        void setStart(const Ice::DoubleSeq& starts);
        void setStartAndGoal(const Ice::DoubleSeq& starts, const Ice::DoubleSeq& goals);
        void setViaPoint(Ice::Double u, const Ice::DoubleSeq& viapoint);
        void removeAllViaPoint();
 
        /// serialze
        void setWeight(const std::vector<std::vector<double>> &weights);
        void setTranslationWeights(const std::vector<std::vector<double>>& weights);
        void setRotationWeights(const std::vector<std::vector<double>>& weights);
        DoubleSeqSeq getWeight();
        std::string serialize(const std::string& mode = "string");
        Ice::DoubleSeq deserialize(const std::string&, const std::string& mode = "string");
 
        ///
        double getCanonicalValue();
        virtual void run(MPInputPtr, MPOutputPtr) = 0;
 
    private:
        void resetVMPType(const std::string& mpTypeString)
        {
            if (mpTypeString != "")
            {
                vmpType = mplib::representation::VMPType::_from_string_nocase(mpTypeString.c_str());
            }
        }
 
    protected:
        MPConfig cfg;
 
        bool isDisturbance = false;
 
        mutable MutexType mpMutex;
        VMPPtr vmp;
        mplib::representation::VMPType vmpType = mplib::representation::VMPType::PrincipalComponent;
 
        std::atomic_bool running = false;
        std::atomic_bool finished = false;
        std::atomic_bool paused = false;
        double canonicalValue = 1.0;
 
        std::atomic_bool goalSetByUser {false};
        /// Note: if mp is not trained, the dimension is not initialized, thus mp->getDim() is undefined, if you set
        /// viapoints you will have memory allocation error. So make sure mp is trained before setting any viapoint.
        std::atomic_bool mpTrained {false};
        mplib::core::DVec targetPoseVec;
        mplib::core::DVec targetPoseVecInTraj;
        std::vector<mplib::representation::MPState> currentState;
        std::vector<std::pair<double, mplib::core::DVec>> userDefinedViaPoints;
    };
 
    using MPPtr = std::shared_ptr<MP>;
 
} /// namespace armarx::control::common::mp