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 <SimoxUtility/json.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/interface/core/BasicTypes.h>
#include <ArmarXCore/interface/serialization/Eigen.h>
 
#include <armarx/control/common/mp/aron/MPConfig.aron.generated.h>
 
#include <mplib/factories/AbstractMPFactory.h>
#include <mplib/factories/MPFactoryCreator.h>
#include <mplib/factories/VMPFactory.h>
#include <mplib/representation/vmp/PrincipalComponentVMP.h>
#include <mplib/representation/vmp/TaskSpacePrincipalComponentVMP.h>
#include <mplib/representation/vmp/VMP.h>
 
namespace armarx::control::common::mp
{
    using namespace arondto;
    using VMPPtr = std::shared_ptr<mplib::representation::vmp::PrincipalComponentVMP>;
    using MutexType = std::recursive_mutex;
    using LockGuardType = std::lock_guard<std::recursive_mutex>;
 
    struct PhaseStopResult
    {
        double phaseStop;
        double mpcFactor;
    };
 
    using DVec = Ice::DoubleSeq;
    using DVecVec = DoubleSeqSeq;
 
    struct MPInput
    {
        virtual ~MPInput()
        {
        }
    };
 
    struct MPOutput
    {
        virtual ~MPOutput()
        {
        }
    };
 
    using MPInputPtr = std::shared_ptr<MPInput>;
    using MPOutputPtr = std::shared_ptr<MPOutput>;
    using MPTrajs = std::vector<MPTraj>;
 
    class MP
    {
    public:
        MP(const MPConfig& c);
        virtual ~MP() = default;
 
        const std::string& getMPName() const;
        const std::string& getClassName() const;
        const std::string& getNodeSetName() const;
        const std::string& getRole() const;
        std::string getGroupName() const;
        const arondto::MPGroup& getGroup() const;
        bool isTaskSpaceMP() const;
 
        /// control
        void start();
        void start(const DVec& goals);
        void start(const DVec& goals, Ice::Double timeDuration);
        void start(const DVec& goals, const DVec& starts, Ice::Double timeDuration);
        void start(const DVec& goals, const DVec& starts);
        void stop();
        void requestStop();
        void pause();
        void resume();
        void reset();
 
        /// status
        bool isFinished();
        bool isRunning();
        bool isFirstRun();
        bool isStopRequested();
        MPConfig getConfig();
 
        /// setting
        void learnFromCSV(const Ice::StringSeq& fileNames = std::vector<std::string>());
        void learnFromTraj(const MPTrajs& trajs = MPTrajs());
        void trainMPFromTraj(std::vector<mplib::core::SampledTrajectory>& trajs);
        void trainMP();
 
        void setDurationSec(Ice::Double timeDuration);
        void setGoal(const DVec& goals);
        void setStart(const DVec& starts);
        std::vector<double> getStartVec();
        void setStartAndGoal(const DVec& starts, const DVec& goals);
        void setViaPoint(Ice::Double u, const DVec& viapoint);
        void removeAllViaPoint();
        bool getStartFromPrevTarget();
 
        DVec prevViapoint;
 
        virtual DVec
        validateViaPoint(const DVec& viapoint)
        {
            return viapoint;
        };
 
        virtual void validateInitialState(const DVec& starts){};
 
        /// serialze
        void setWeight(const DVecVec& weights);
        void setTranslationWeights(const DVecVec& weights);
        void setRotationWeights(const DVecVec& weights);
        DVecVec getWeight();
        std::string serialize(const std::string& mode = "string");
        DVec deserialize(const std::string&, const std::string& mode = "string");
 
        ///
        double getCanonicalValue();
        virtual void run(MPInputPtr, MPOutputPtr, const PhaseStopResult& phaseStop) = 0;
 
        virtual void computePhaseStop(const MPInputPtr input,
                                      const MPOutputPtr output,
                                      PhaseStopResult& phaseStop){};
 
    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;
        std::atomic_bool stopRequested = false;
        double canonicalValue = 1.0;
        std::atomic_bool firstRun = true;
 
        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};
        DVec targetPoseVec;
        DVec targetPoseVecInTraj;
        std::vector<mplib::representation::MPState> currentState;
        std::vector<std::pair<double, DVec>> userDefinedViaPoints;
 
        armarx::DateTime mpStartTimeSec;
    };
 
    using MPPtr = std::shared_ptr<MP>;
 
} // namespace armarx::control::common::mp