MP.cpp

Go to the documentation of this file.

#include "MP.h"
 
#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 <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include "../utils.h"
#include <mplib/core/SystemState.h>
 
namespace armarx::control::common::mp
{
 
    //MP::MP(const std::string &name, nlohmann::json &userConfig, nlohmann::json &defaultConfig)
    MP::MP(const MPConfig& c)
    {
 
        cfg = c;
        mplib::factories::VMPFactory mpfactory;
        mpfactory.addConfig("kernelSize", c.kernelSize);
 
        resetVMPType(c.mpTypeString);
        // ARMARX_INFO << "Creating mp with name: " << c.name << "\n" << VAROUT(c.className) << "\n" << VAROUT(c.mpTypeString) << "\n" << VAROUT(c.mpMode) << "\n" << VAROUT(c.mpStyle);
        std::shared_ptr<mplib::representation::AbstractMovementPrimitive> mp =
            mpfactory.createMP(vmpType);
        vmp = std::dynamic_pointer_cast<mplib::representation::vmp::PrincipalComponentVMP>(mp);
 
        if (cfg.viaPoints.size() > 0)
        {
            ARMARX_INFO << "-- adding via points: " << cfg.viaPoints.size();
            int index = 0;
            for (const auto& vp : cfg.viaPoints)
            {
                ARMARX_INFO << "---- (" << index << ") canonical value: " << vp.canonicalValue
                            << "  ---  " << common::dVecToString(vp.viaPointValue);
                ARMARX_CHECK_GREATER_EQUAL(vp.canonicalValue, 0);
                ARMARX_CHECK_LESS_EQUAL(vp.canonicalValue, 1);
                userDefinedViaPoints.push_back({vp.canonicalValue, vp.viaPointValue});
                index++;
            }
        }
    }
 
    const std::string&
    MP::getMPName() const
    {
        return cfg.name;
    }
 
    const std::string&
    MP::getClassName() const
    {
        return cfg.className;
    }
 
    const std::string&
    MP::getNodeSetName() const
    {
        return cfg.nodeSetName;
    }
 
    const std::string&
    MP::getRole() const
    {
        return cfg.role;
    }
 
    std::string
    MP::getGroupName() const
    {
        return cfg.group.toString();
    }
 
    const arondto::MPGroup&
    MP::getGroup() const
    {
        return cfg.group;
    }
 
    bool
    MP::isTaskSpaceMP() const
    {
        return cfg.role == "taskspace" or cfg.role == "object";
    }
 
    void
    MP::start()
    {
        start(std::vector<double>());
    }
 
    void
    MP::start(const DVec& goals)
    {
        start(goals, std::vector<double>());
    }
 
    void
    MP::start(const DVec& goals, Ice::Double timeDuration)
    {
        cfg.durationSec = timeDuration;
        start(goals);
    }
 
    void
    MP::setDurationSec(Ice::Double timeDuration)
    {
        cfg.durationSec = timeDuration;
    }
 
    void
    MP::start(const DVec& goals, const DVec& starts, Ice::Double timeDuration)
    {
        cfg.durationSec = timeDuration;
        start(goals, starts);
    }
 
    void
    MP::start(const DVec& goals, const DVec& starts)
    {
        ARMARX_INFO << "---- start " << cfg.name;
        canonicalValue = 1.0;
        /// parse user defined viapoints from configuration file
        for (const auto& vp : userDefinedViaPoints)
        {
            setViaPoint(vp.first, vp.second);
            //        ARMARX_INFO << "viapoint at " << vp.first << " is " << common::dVecToString(vp.second);
        }
 
        /// overwrite goals and starts if available
        setStartAndGoal(starts, goals);
 
        /// prepare execution
        LockGuardType guard{mpMutex};
        vmp->prepareExecution(targetPoseVec, currentState);
        auto vps = vmp->getViaPoints();
        ARMARX_INFO << "---- Your MP have: ";
        int index = 0;
        for (const auto& v : vps)
        {
            ARMARX_INFO << "------ (" << index << ") viapoint at " << v.first << " --- "
                        << common::dVecToString(
                               mplib::core::SystemState::convertStatesToArray(v.second, 0));
            index++;
        }
 
        /// set flag
        running.store(true);
        stopRequested.store(false);
        resume();
        ARMARX_INFO << "---- MP started: " << cfg.name;
    }
 
    void
    MP::stop()
    {
        running.store(false);
        stopRequested.store(false);
    }
 
    void
    MP::requestStop()
    {
        stopRequested.store(true);
    }
 
    void
    MP::pause()
    {
        paused.store(true);
    }
 
    void
    MP::resume()
    {
        paused.store(false);
    }
 
    void
    MP::reset()
    {
        if (running.load())
        {
            ARMARX_INFO << "cannot reset a running mp, please stop it first.";
        }
        else
        {
            userDefinedViaPoints.clear();
            targetPoseVec.clear();
            targetPoseVecInTraj.clear();
            currentState.clear();
            removeAllViaPoint();
            goalSetByUser.store(false);
            mpTrained.store(false);
            prevViapoint.clear();
        }
    }
 
    bool
    MP::isFinished()
    {
        return !running.load();
    }
 
    bool
    MP::isRunning()
    {
        return running.load();
    }
 
    bool
    MP::isFirstRun()
    {
        return firstRun.load();
    }
 
    bool
    MP::isStopRequested()
    {
        return stopRequested.load();
    }
 
    MPConfig
    MP::getConfig()
    {
        return cfg;
    }
 
    void
    MP::learnFromTraj(const MPTrajs& mpTrajs)
    {
        ARMARX_INFO << "-- Train MP '" << cfg.name << "' from trajectories";
        MPTrajs trajList;
        if (mpTrajs.empty())
        {
            ARMARX_INFO << "---- using default trajectories from the MP configuration";
            trajList = cfg.trajectoryList;
        }
        else
        {
            ARMARX_INFO << "---- use user-provided traj";
            trajList = mpTrajs;
        }
 
        std::vector<mplib::core::SampledTrajectory> trajs;
        for (const auto& mpTraj : trajList)
        {
            std::map<double, std::vector<double>> trajMap;
            for (int i = 0; i < mpTraj.time.size(); ++i)
            {
                auto& row_vector = mpTraj.traj.row(i);
                std::vector<double> vec(row_vector.data(), row_vector.data() + row_vector.size());
                trajMap.emplace(mpTraj.time(i), vec);
            }
            mplib::core::SampledTrajectory traj(trajMap);
            trajs.push_back(traj);
        }
        ARMARX_TRACE;
        trainMPFromTraj(trajs);
    }
 
    void
    MP::learnFromCSV(const Ice::StringSeq& fileNames)
    {
        ARMARX_INFO << "-- Train MP '" << cfg.name << "' from csv files";
        std::vector<std::string> fileList;
        if (fileNames.empty())
        {
            ARMARX_INFO << "---- using default trajectory list: " << cfg.fileList[0];
            fileList = cfg.fileList;
        }
        else
        {
            fileList = fileNames;
        }
 
        std::vector<mplib::core::SampledTrajectory> trajs;
        for (auto& file : fileList)
        {
            mplib::core::SampledTrajectory traj;
            traj.readFromCSVFile(file);
            trajs.push_back(traj);
        }
        trainMPFromTraj(trajs);
    }
 
    void
    MP::trainMPFromTraj(std::vector<mplib::core::SampledTrajectory>& trajs)
    {
        if (cfg.regressionModel == "gpr")
        {
            vmp->setBaseFunctionApproximator(
                std::make_unique<mplib::math::function_approximation::GaussianProcessRegression<
                    mplib::math::kernel::SquaredExponentialCovarianceFunction>>(
                    0.01, 0.1, 0.000001));
            for (auto& traj : trajs)
            {
                traj = mplib::core::SampledTrajectory::downSample(traj, 100);
            }
        }
        ARMARX_TRACE;
        vmp->learnFromTrajectories(trajs);
        ARMARX_TRACE;
 
        currentState.clear();
        targetPoseVecInTraj.clear();
 
        ARMARX_INFO << "---- Your trajectory has " << VAROUT(trajs[0].dim());
        for (size_t i = 0; i < trajs[0].dim(); i++)
        {
            mplib::representation::MPState state(trajs[0].begin()->getPosition(i),
                                                 trajs[0].begin()->getDeriv(i, 1));
            currentState.push_back(state);
            targetPoseVecInTraj.push_back(trajs[0].rbegin()->getPosition(i));
        }
        ARMARX_TRACE;
        mpTrained.store(true);
        ARMARX_INFO << "---- MP trained";
    }
 
    void
    MP::trainMP()
    {
        if (not cfg.fileList.empty())
        {
            learnFromCSV();
        }
        else if (not cfg.trajectoryList.empty())
        {
            learnFromTraj();
        }
        else
        {
            ARMARX_ERROR
                << cfg.name
                << ": You don't provide any trajectory files (path on the robot) nor trajectories "
                   "in "
                   "type std::vector<armarx::control::common::mp::arondto::MPTraj> in your "
                   "configuration file. \n"
                << "If you intended train MPs by providing files or "
                   "trajectories on the fly, consider using \n"
                << "ctrl.learnFromCSV(Ice::StringSeq& fileNames) or \n"
                << "ctrl.learnFromCSVlearnFromTrajs(armarx::aron::data::dto::DictPtr& dict)";
        }
    }
 
    void
    MP::setGoal(const DVec& goals)
    {
        ARMARX_INFO << "---- Reset MP goal: " << common::dVecToString(goals);
        if (!goals.empty())
        {
            goalSetByUser.store(true);
            targetPoseVec = goals;
        }
        else if (!targetPoseVec.empty())
        {
            ARMARX_INFO
                << "---- user specified empty goal, try to use goals in your configuration file:\n"
                << common::dVecToString(targetPoseVec);
        }
        else if (!targetPoseVecInTraj.empty())
        {
            ARMARX_INFO
                << "---- user specified empty goal, try to use goals learned from trajectory:\n"
                << common::dVecToString(targetPoseVecInTraj);
            targetPoseVec = targetPoseVecInTraj;
        }
        else
        {
            ARMARX_WARNING << "---- user specified empty goal";
        }
    }
 
    void
    MP::setStart(const DVec& starts)
    {
        ARMARX_INFO << "---- Reset MP start: " << common::dVecToString(starts);
        if (!starts.empty())
        {
            ARMARX_CHECK_EQUAL(starts.size(), currentState.size())
                << ": size of starts and learned mp are not consistent";
            for (size_t i = 0; i < currentState.size(); i++)
            {
                currentState[i].pos = starts[i];
                currentState[i].vel = 0.0;
            }
        }
        else
        {
            std::vector<double> learnedStart;
            for (size_t i = 0; i < currentState.size(); i++)
            {
                learnedStart.push_back(currentState[i].pos);
            }
            ARMARX_INFO << "---- user doesn't define start point, fall back to start point "
                           "learned from trajectory: "
                        << common::dVecToString(learnedStart);
        }
    }
 
    std::vector<double>
    MP::getStartVec()
    {
        const auto& startState = vmp->getStartState();
        std::vector<double> mpStartVec;
        for (const auto& state : startState)
        {
            mpStartVec.push_back(state.pos);
        }
        return mpStartVec;
    }
 
    bool
    MP::getStartFromPrevTarget()
    {
        return cfg.startFromPrevTarget;
    }
 
    void
    MP::setStartAndGoal(const DVec& starts, const DVec& goals)
    {
        setStart(starts);
        setGoal(goals);
    }
 
    void
    MP::setViaPoint(Ice::Double canVal, const DVec& viapoint)
    {
        if (mpTrained.load())
        {
            prevViapoint = validateViaPoint(viapoint);
            ARMARX_INFO << "====> Adding via point " << common::dVecToString(prevViapoint);
            LockGuardType guard{mpMutex};
            if (canVal <= vmp->getUmin())
            {
                setGoal(prevViapoint);
            }
            else if (canVal >= 1.0 - vmp->getUmin())
            {
                setStart(prevViapoint);
            }
            else
            {
                vmp->setViaPoint(canVal, prevViapoint);
            }
        }
        else
        {
            ARMARX_ERROR << "    -- mp is not trained yet, potential memory allocation error! "
                            "Please train mp before setting any via points";
        }
    }
 
    void
    MP::removeAllViaPoint()
    {
        LockGuardType guard{mpMutex};
        vmp->removeViaPoints();
    }
 
    double
    MP::getCanonicalValue()
    {
        return canonicalValue;
    }
 
    /// serialze
    void
    MP::setWeight(const DVecVec& weights)
    {
        vmp->setWeights(weights);
    }
 
    DVecVec
    MP::getWeight()
    {
        return vmp->getWeights();
    }
 
    std::string
    MP::serialize(const std::string& mode)
    {
        //    std::stringstream ss;
        //    if (mode == "string")
        //    {
        //        boost::archive::text_oarchive oa{ss};
        //        oa << vmp.get();
        //    }
        //    else if (mode == "xml")
        //    {
        //        boost::archive::xml_oarchive oa(ss);
        //        oa << vmp.get();
        //    }
        //    return ss.str();
        return "";
    }
 
    DVec
    MP::deserialize(const std::string& mpString, const std::string& mode)
    {
        //    std::stringstream ss;
        //    ss.str(mpString);
        //    mplib::representation::vmp::PrincipalComponentVMP* dmpPointer;
        //    if (mode == "string")
        //    {
        //        boost::archive::text_iarchive ia{ss};
        //        ia >> dmpPointer;
        //    }
        //    else if (mode == "xml")
        //    {
        //        boost::archive::xml_iarchive ia(ss);
        //        ia >> dmpPointer;
        //    }
        //    else
        //    {
        //        ARMARX_ERROR << VAROUT(mode) << " unsupported! please use 'string' or 'xml' mode";
        //    }
        //    vmp.reset(dmpPointer);
        return vmp->getGoals();
    }
 
} // namespace armarx::control::common::mp