MPPool.cpp

Go to the documentation of this file.

#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <SimoxUtility/json.h>
#include "MPPool.h"
#include "TSMP.h"
#include "JSMP.h"
#include "KeypointsMP.h"
#include "../utils.h"
 
 
 
namespace armarx::control::common::mp
{
 
void MPPool::createMPs(const MP::MPListConfig& mpListConfig)
{
    ARMARX_INFO << "----------------------------------- creating MPs -------------------------------------------------";
    ARMARX_IMPORTANT << "in MPPool, we have " << mpListConfig.mpList.size() << " MPs";
 
    for (const MP::MPConfig& c: mpListConfig.mpList)
    {
        auto className = mpClass::_from_string_nocase(c.className.c_str());
        ARMARX_IMPORTANT << " -- creating MP with name " << c.name << " and type: " << c.className << VAROUT(c.mpTypeString) << VAROUT(c.mpMode) << VAROUT(c.mpStyle);
        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;
        }
    }
    ARMARX_INFO << "---------------------------------- All MPs created -----------------------------------------------";
}
 
void MPPool::reconfigureMPs(const MP::MPListConfig& mpListConfig)
{
    mps.clear();
    createMPs(mpListConfig);
}
 
void MPPool::runMPs()
{
    for (auto& mp: mps)
    {
        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::start(const std::string& mpName, const Ice::DoubleSeq& startVec, const Ice::DoubleSeq& goalVec, Ice::Double timeDuration, const Ice::Current &iceCurrent)
{
    if (mpName == "all")
    {
        ARMARX_IMPORTANT << "to start all MPs at once, it's up to the user to configure the start, goal and timeduration properly before this call.";
        for (auto& mp: mps)
        {
            mp.second.mp->start();
        }
    }
    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";
}
 
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::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::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::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::isFinished(const std::string& mpName, const Ice::Current &)
{
    if (mpName == "all")
    {
        for (auto& mp: mps)
        {
            if (!mp.second.mp->isFinished())
            {
                return false;
            }
        }
        return true;
    }
    else
    {
        auto search = mps.find(mpName);
        if (search != mps.end())
        {
            return search->second.mp->isFinished();
        }
        else
        {
            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)
{
    ARMARX_INFO << "--------------------------------------- Train MPs ----------------------------------------------------";
    for (auto& mp: mps)
    {
        mp.second.mp->learnFromCSV();
    }
    ARMARX_INFO << "--------------------------------------- done ----------------------------------------------------";
}
 
void MPPool::setGoal(const Ice::DoubleSeq &goals, const Ice::Current &iceCurrent)
{
 
}
 
void MPPool::setStartAndGoal(const Ice::DoubleSeq &starts, const Ice::DoubleSeq &goals, const Ice::Current &iceCurrent)
{
    for (auto& mp: mps)
    {
        mp.second.mp->setStartAndGoal(starts, goals);
    }
}
 
void MPPool::setViaPoint(Ice::Double u, const Ice::DoubleSeq &viapoint, const Ice::Current &iceCurrent)
{
 
}
 
void MPPool::removeAllViaPoint(const Ice::Current &iceCurrent)
{
 
}
 
//void MPPool::setWeight(const Ice::DoubleSeqSeq& weights, const Ice::Current &iceCurrent)
//{
 
//}
 
std::string MPPool::serialize(const Ice::Current &iceCurrent)
{
    return "";
}
 
Ice::DoubleSeq 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;
}
 
 
} /// namespace armarx::control::common::mp