MPPool.cpp

Go to the documentation of this file.

#include "MPPool.h"
 
#include <SimoxUtility/json.h>
#include <VirtualRobot/Nodes/RobotNode.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <armarx/control/common/aron_conversions.h>
#include <armarx/control/common/mp/aron/MPConfig.aron.generated.h>
 
#include "../utils.h"
#include "JSMP.h"
#include "KeypointsMP.h"
#include "TSMP.h"
 
namespace armarx::control::common::mp
{
 
    void
    MPPool::createMPs(const MPListConfig& mpListConfig)
    {
        auto _start = IceUtil::Time::now().toSecondsDouble();
        ARMARX_INFO << "----------------------------------- creating MPs "
                       "-------------------------------------------------";
        ARMARX_INFO << " -- in MPPool, we have " << mpListConfig.mpList.size() << " MPs";
 
        int index = 0;
        for (const MPConfig& c : mpListConfig.mpList)
        {
            auto className = mpClass::_from_string_nocase(c.className.c_str());
            ARMARX_INFO << "-- (" << index << ") creating MP with name " << c.name
                        << "\n---- class: " << c.className << "\n---- type: " << c.mpTypeString
                        << "\n---- mode: " << c.mpMode << "\n---- style: " << c.mpStyle;
            index++;
            switch (className)
            {
                case mpClass::JSMP:
                {
                    JSMPPtr jsmp(new JSMP(c));
                    JSMPInputPtr in(new JSMPInput());
                    JSMPOutputPtr out(new JSMPOutput());
                    mps.insert({c.name, {jsmp, in, out}});
                    break;
                }
                case mpClass::JSVelMP:
                    break;
                case mpClass::TSMP:
                {
                    TSMPPtr tsmp(new TSMP(c));
                    TSMPInputPtr in(new TSMPInput());
                    TSMPOutputPtr out(new TSMPOutput());
                    mps.insert({c.name, {tsmp, in, out}});
                    break;
                }
                case mpClass::KeypointsMP:
                {
                    KeypointsMPPtr tsmp(new KeypointsMP(c));
                    KeypointsMPInputPtr in(new KeypointsMPInput());
                    KeypointsMPOutputPtr out(new KeypointsMPOutput());
                    mps.insert({c.name, {tsmp, in, out}});
                    break;
                }
                case mpClass::TSVelMP:
                    break;
                default:
                    ARMARX_WARNING << "unsupported MP class: " << c.className
                                   << "\nsupported names include: "
                                      "JSMP, JSVelMP, TSMP, TSVelMP, KeypointsMP";
                    break;
            }
        }
        auto _duration = IceUtil::Time::now().toSecondsDouble() - _start;
        ARMARX_INFO << "---------------------------------- All MPs created in " << _duration
                    << " sec "
                       "-----------------------------------------------";
    }
 
    void
    MPPool::reconfigureMPs(const MPListConfig& mpListConfig)
    {
        mps.clear();
        createMPs(mpListConfig);
    }
 
    void
    MPPool::runMPs(const bool rtSafe)
    {
        for (auto& mp : mps)
        {
            if (rtSafe or mp.second.mp->isFirstRun())
            {
                mp.second.mp->run(mp.second.input, mp.second.output);
            }
        }
    }
 
    std::string
    MPPool::getNames(const Ice::Current& iceCurrent)
    {
        std::string allMPs = "";
        for (auto& mp : mps)
        {
            allMPs = allMPs + mp.second.mp->getMPName() + ", ";
        }
        return allMPs;
    }
 
    void
    MPPool::set(const ::armarx::StringDoubleSeqMap& startVec,
                const ::armarx::StringDoubleSeqMap& goalVec,
                const ::armarx::StringDoubleMap& durationSec,
                const ::Ice::Current&)
    {
        for (auto& pair : mps)
        {
            auto it = durationSec.find(pair.first);
            auto is = startVec.find(pair.first);
            auto ig = goalVec.find(pair.first);
 
            if (it != durationSec.end())
            {
                if (it->second > 0.0)
                {
                    pair.second.mp->setDurationSec(it->second);
                }
            }
            if (is != startVec.end())
            {
                pair.second.mp->setStart(is->second);
            }
            if (ig != goalVec.end())
            {
                pair.second.mp->setGoal(ig->second);
            }
        }
    }
 
    void
    MPPool::startAll(const Ice::Current& iceCurrent)
    {
        start();
    }
 
    void
    MPPool::start(const std::string& mpName,
                  const DVec& startVec,
                  const DVec& goalVec,
                  Ice::Double timeDuration,
                  const Ice::Current& iceCurrent)
    {
        auto start = IceUtil::Time::now().toSecondsDouble();
        if (mpName == "all")
        {
            ARMARX_INFO << "--------------------------------------- start all MPs "
                           "---------------------------------------";
            ARMARX_INFO << " -- To start all MPs at once, it's up to the user to configure the "
                           "start, goal and timeduration properly before this call.";
            int index = 0;
            for (auto& mp : mps)
            {
                ARMARX_INFO << " -- (" << index << ")-th MP: ";
                mp.second.mp->start();
                index++;
            }
            ARMARX_INFO << " -- all MPs started.";
        }
        else
        {
            auto search = mps.find(mpName);
            if (search != mps.end())
            {
                if (timeDuration < 0.0)
                {
                    search->second.mp->start(goalVec, startVec);
                }
                else
                {
                    search->second.mp->start(goalVec, startVec, timeDuration);
                }
            }
            else
            {
                ARMARX_ERROR << mpName
                             << " is not in the MP pool. Please check your configuration file";
            }
        }
        // ARMARX_IMPORTANT << "starting mp" << VAROUT(mpName) << "with start: " << dVecToString(startVec) << " and goal: " << dVecToString(goalVec) << " for " << timeDuration << " seconds";
        auto duration = IceUtil::Time::now().toSecondsDouble() - start;
        ARMARX_INFO << "All mp started in " << duration << " sec.";
    }
 
    void
    MPPool::stopAll(const Ice::Current& iceCurrent)
    {
        stop();
    }
 
    void
    MPPool::requestStop(const std::string& mpName, const Ice::Current&)
    {
        if (mpName == "all")
        {
            for (auto& mp : mps)
            {
                mp.second.mp->requestStop();
            }
        }
        else
        {
            auto search = mps.find(mpName);
            if (search != mps.end())
            {
                search->second.mp->requestStop();
            }
            else
            {
                ARMARX_ERROR << mpName
                             << " is not in the MP pool. Please check your configuration file";
            }
        }
    }
 
    void
    MPPool::stop(const std::string& mpName, const Ice::Current&)
    {
        if (mpName == "all")
        {
            for (auto& mp : mps)
            {
                mp.second.mp->stop();
            }
        }
        else
        {
            auto search = mps.find(mpName);
            if (search != mps.end())
            {
                search->second.mp->stop();
            }
            else
            {
                ARMARX_ERROR << mpName
                             << " is not in the MP pool. Please check your configuration file";
            }
        }
    }
 
    void
    MPPool::pauseAll(const Ice::Current& iceCurrent)
    {
        pause();
    }
 
    void
    MPPool::pause(const std::string& mpName, const Ice::Current&)
    {
        if (mpName == "all")
        {
            for (auto& mp : mps)
            {
                mp.second.mp->pause();
            }
        }
        else
        {
            auto search = mps.find(mpName);
            if (search != mps.end())
            {
                search->second.mp->pause();
            }
            else
            {
                ARMARX_ERROR << mpName
                             << " is not in the MP pool. Please check your configuration file";
            }
        }
    }
 
    void
    MPPool::resumeAll(const Ice::Current& iceCurrent)
    {
        resume();
    }
 
    void
    MPPool::resume(const std::string& mpName, const Ice::Current&)
    {
        if (mpName == "all")
        {
            for (auto& mp : mps)
            {
                mp.second.mp->resume();
            }
        }
        else
        {
            auto search = mps.find(mpName);
            if (search != mps.end())
            {
                search->second.mp->resume();
            }
            else
            {
                ARMARX_ERROR << mpName
                             << " is not in the MP pool. Please check your configuration file";
            }
        }
    }
 
    void
    MPPool::resetAll(const Ice::Current& iceCurrent)
    {
        reset();
    }
 
    void
    MPPool::reset(const std::string& mpName, const Ice::Current&)
    {
        if (mpName == "all")
        {
            for (auto& mp : mps)
            {
                mp.second.mp->reset();
            }
        }
        else
        {
            auto search = mps.find(mpName);
            if (search != mps.end())
            {
                search->second.mp->reset();
            }
            else
            {
                ARMARX_ERROR << mpName
                             << " is not in the MP pool. Please check your configuration file";
            }
        }
    }
 
    bool
    MPPool::isFinishedAll(const Ice::Current& iceCurrent)
    {
        return isFinished();
    }
 
    bool
    MPPool::isFinished(const std::string& mpName, const Ice::Current&)
    {
        if (mpName == "all")
        {
            return std::all_of(mps.begin(),
                               mps.end(),
                               [](const auto& pair) { return pair.second.mp->isFinished(); });
        }
 
        auto it = mps.find(mpName);
        if (it != mps.end())
        {
            return it->second.mp->isFinished();
        }
 
        ARMARX_ERROR << mpName << " is not in the MP pool. Please check your configuration file";
        return true;
    }
 
    void
    MPPool::learnFromCSV(const Ice::StringSeq& fileNames, const Ice::Current& iceCurrent)
    {
        if (fileNames.size() > 0)
        {
            ARMARX_WARNING << "Be aware that the fileNames ice parameter is not yet used. "
                           << "Instead, set the file names in the MPConfig parameter of your "
                           << "previous ice interface call.";
        }
        ARMARX_INFO << "--------------------------------------- Train MPs "
                       "----------------------------------------------------";
        for (auto& mp : mps)
        {
            mp.second.mp->learnFromCSV();
        }
        isMPReady.store(true);
        ARMARX_INFO << "--------------------------------------- done "
                       "----------------------------------------------------";
    }
 
    void
    MPPool::learnFromTrajs(const ::armarx::aron::data::dto::DictPtr& dict,
                           const Ice::Current& iceCurrent)
    {
        auto start = IceUtil::Time::now().toSecondsDouble();
        ARMARX_INFO << "--------------------------------------- Train MPs "
                       "----------------------------------------------------";
 
        // Convert parameter of ice interface call to business object
        MultiMPTrajs trajsDict;
        trajsDict.fromAron(dict);
        //        std::map<std::string, std::vector<arondto::MPTraj>> dto =
        //            arondto::MultiMPTrajs::FromAron(dict).data;
        //        //        armarx::aron::fromAron<std::vector<arondto::MPTraj>, MPTrajs>(arondto::MultiMPTrajs::FromAron(dict), bo);
        //        armarx::aron::fromAron<std::string, std::vector<arondto::MPTraj>, std::string, MPTrajs>(
        //            dto, trajsDict);
 
        //        MultiMPTrajs trajsDict = ::armarx::fromAronDict<arondto::MultiMPTrajs, MultiMPTrajs>(dict);
 
        if (trajsDict.data.empty())
        {
            ARMARX_INFO << "Learning from trajectories specified in MPConfig(s) rather than as an "
                           "ice parameter";
            for (const auto& [mpName, mpInputOutput] : mps)
            {
                const auto& mpConfig = mpInputOutput.mp->getConfig();
                ARMARX_CHECK(not mpConfig.trajectoryList.empty())
                    << "For MP " << mpName << ": No trajectories provided as parameter, "
                    << "and not trajectories specified in MPConfig.";
                mpInputOutput.mp->learnFromTraj(mpConfig.trajectoryList);
            }
        }
        else
        {
            for (const auto& [mpName, trajs] : trajsDict.data)
            {
                // Learn the MP identified by mpName from the trajectory list stored in
                // trajsDict.at(mpName)
                ARMARX_CHECK(mps.count(mpName))
                    << "Trajectories provided for unknown MP " << mpName;
                ARMARX_CHECK(not trajs.empty())
                    << "Empty list of trajectories provided for MP " << mpName;
                mps.at(mpName).mp->learnFromTraj(trajs);
            }
        }
        // mark whole MP pool as ready
        isMPReady.store(true);
 
        auto duration = IceUtil::Time::now().toSecondsDouble() - start;
        ARMARX_INFO << "--------------------------------------- done in " << duration
                    << " sec "
                       "----------------------------------------------------";
    }
 
    void
    MPPool::trainMP(const Ice::Current& iceCurrent)
    {
        auto start = IceUtil::Time::now().toSecondsDouble();
        for (auto& mp : mps)
        {
            mp.second.mp->trainMP();
        }
        isMPReady.store(true);
        auto duration = IceUtil::Time::now().toSecondsDouble() - start;
        ARMARX_INFO << "training finished in " << duration << " sec.";
    }
 
    void
    MPPool::setGoal(const DVec& goals, const Ice::Current& iceCurrent)
    {
    }
 
    void
    MPPool::setStartAndGoal(const DVec& starts, const DVec& goals, const Ice::Current& iceCurrent)
    {
        for (auto& mp : mps)
        {
            mp.second.mp->setStartAndGoal(starts, goals);
        }
    }
 
    void
    MPPool::setViaPoint(Ice::Double u, const DVec& viapoint, const Ice::Current& iceCurrent)
    {
    }
 
    void
    MPPool::removeAllViaPoint(const Ice::Current& iceCurrent)
    {
    }
 
    //void MPPool::setWeight(const DVecSeq& weights, const Ice::Current &iceCurrent)
    //{
 
    //}
 
    std::string
    MPPool::serialize(const Ice::Current& iceCurrent)
    {
        return "";
    }
 
    DVec
    MPPool::deserialize(const std::string&, const Ice::Current& iceCurrent)
    {
        std::vector<double> test{0.1, 0.1};
        return test;
    }
 
    Ice::Double
    MPPool::getCanVal(const std::string& mpName, const Ice::Current& iceCurrent)
    {
        auto search = mps.find(mpName);
        if (search != mps.end())
        {
            return search->second.mp->getCanonicalValue();
        }
        else
        {
            ARMARX_ERROR << mpName
                         << " is not in the MP pool. Please check your configuration file";
            return true;
        }
    }
 
    bool
    MPPool::getMPEnabled(const Ice::Current&)
    {
        return true;
    }
 
    void
    MPPool::updateMPConfig(const ::armarx::aron::data::dto::DictPtr& dto,
                           const Ice::Current& iceCurrent)
    {
        ARMARX_WARNING << "You should overwrite this method in your concrete MP controllers, and "
                          "call resetMPs(dto, rnsMap)";
    }
 
    bool
    MPPool::resetMPs(const ::armarx::aron::data::dto::DictPtr& dto,
                     const std::map<std::string, VirtualRobot::RobotNodeSetPtr>& rnsMap)
    {
        std::scoped_lock lock(mtx_mps);
        if (not isFinishedAll())
        {
            ARMARX_WARNING << "MP is not finished yet, cannot reset MP\n"
                              "If you want to force reset, please call \n\n"
                              "      ctrl->stop('all')    \n\n"
                              "before you continue";
            return false;
        }
 
        auto configData = MPListConfig::FromAron(dto);
        isMPReady.store(false);
        while (mpTaskRunning.load())
        {
            usleep(1000);
        }
 
        for (const auto& _mp : configData.mpList)
        {
            if (rnsMap.find(_mp.nodeSetName) == rnsMap.end())
            {
                ARMARX_WARNING << "You have to make sure the RobotNodeSet " << _mp.nodeSetName
                               << " in your MP config "
                               << "corresponds to one of the RobotNodeSet in the controllers";
                return false;
            }
        }
        mpConfig = configData;
        reconfigureMPs(mpConfig);
 
        reInitMPInputOutputData(rnsMap);
        ARMARX_INFO << "-- successfully updated MP config.";
        return true;
    }
 
    ::armarx::aron::data::dto::DictPtr
    MPPool::getMPConfig(const Ice::Current& iceCurrent)
    {
        return mpConfig.toAronDTO();
    }
 
    void
    MPPool::reInitMPInputOutputData(
        const std::map<std::string, VirtualRobot::RobotNodeSetPtr>& rnsMap)
    {
        ARMARX_INFO << "-- reconfigure MP: reinitialize the mp input output, as well as the rt "
                       "related buffer values";
        int index = 0;
        for (auto& _mp : mps)
        {
            //            const auto& nodeSetName = ;
            //            ARMARX_CHECK_EQUAL(nodeSetName, limb.at(nodeSetName)->kinematicChainName);
            VirtualRobot::RobotNodeSetPtr rns = rnsMap.at(_mp.second.mp->getNodeSetName());
 
            if (_mp.second.mp->getRole() == "taskspace")
            {
                Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
                ARMARX_INFO << "---- (" << index << ") init input output data for "
                            << _mp.second.mp->getMPName();
                std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input)->pose = currentPose;
                std::dynamic_pointer_cast<mp::TSMPInput>(_mp.second.input)->vel.setZero();
                std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output)->pose = currentPose;
                std::dynamic_pointer_cast<mp::TSMPOutput>(_mp.second.output)->vel.setZero();
            }
            else if (_mp.second.mp->getRole() == "nullspace")
            {
                ARMARX_INFO << "---- (" << index << ") init input output data for "
                            << _mp.second.mp->getMPName() << " with "
                            << rns->getJointValues().size() << " joints";
                std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angleRadian =
                    rns->getJointValuesEigen();
                std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angularVel =
                    Eigen::VectorXf::Zero(rns->getJointValues().size());
                std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angleRadian =
                    rns->getJointValuesEigen();
                std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angularVel =
                    Eigen::VectorXf::Zero(rns->getJointValues().size());
            }
            else if (_mp.second.mp->getRole() == "hand")
            {
                ARMARX_INFO << "---- (" << index << ") init input output data for "
                            << _mp.second.mp->getMPName() << " with "
                            << rns->getJointValues().size() << " joints";
                std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angleRadian =
                    rns->getJointValuesEigen();
                std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angularVel =
                    Eigen::VectorXf::Zero(rns->getJointValues().size());
                std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angleRadian =
                    rns->getJointValuesEigen();
                std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angularVel =
                    Eigen::VectorXf::Zero(rns->getJointValues().size());
            }
            index++;
        }
    }
 
} // namespace armarx::control::common::mp