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/coordination/CoordinationCore.h>
#include <armarx/control/common/coordination/SyncCoordination.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_VERBOSE << " -- in MPPool, we have " << mpListConfig.mpList.size() << " MPs";
        phaseStops_.clear();
 
        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
                        << "\n---- group: " << c.group.toString()
                        << (c.enablePhaseStop ? "\n---- phase stop active " : "");
            index++;
            phaseStops_.try_emplace(c.group, PhaseStopResult{});
            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;
        std::string allGroups;
        for (auto& pair : phaseStops_)
        {
            if (not allGroups.empty())
            {
                allGroups += ", ";
            }
            allGroups += pair.first.toString();
        }
        ARMARX_VERBOSE << "-- created phase stop variables for groups: " << allGroups;
        ARMARX_VERBOSE << "---------------------------------- All MPs created in " << duration
                       << " sec "
                          "-----------------------------------------------";
    }
 
    void
    MPPool::reconfigureMPs(const MPListConfig& mpListConfig)
    {
        mps.clear();
        createMPs(mpListConfig);
    }
 
    void
    MPPool::runMPs(const bool rtSafe)
    {
        //        std::map<std::string, PhaseStopResult> phaseStops;
        /// Since there may be MPs for TCP, posture and fingers, we group those for one arm to use
        /// the same phase stop signal, usually it is a good choice to use the Task Space MP to
        /// compute the phase stop signal and propagate this to other mps in the same group
        /// Compute phase stop
        for (auto& mp : mps)
        {
            if ((rtSafe or mp.second.mp->isFirstRun()) and mp.second.mp->isTaskSpaceMP())
            {
                auto& phaseStop = phaseStops_.at(mp.second.mp->getGroup());
                mp.second.mp->computePhaseStop(mp.second.input, mp.second.output, phaseStop);
            }
        }
 
        /// propagate phase top to other mps
        for (auto& mp : mps)
        {
            if (rtSafe or mp.second.mp->isFirstRun())
            {
                auto& phaseStop = phaseStops_.at(mp.second.mp->getGroup());
                mp.second.mp->run(mp.second.input, mp.second.output, phaseStop);
            }
        }
    }
 
    std::string
    MPPool::getNames(const Ice::Current& /*iceCurrent*/)
    {
        std::string allMPs = "";
        for (const auto& mp : mps)
        {
            if (!allMPs.empty())
            {
                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& /*unused*/)
    {
        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_VERBOSE << "--------------------------------------- start all MPs "
                              "---------------------------------------";
            ARMARX_VERBOSE << " -- 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_VERBOSE << " -- (" << index << ")-th MP: ";
                mp.second.mp->start();
                index++;
            }
            ARMARX_VERBOSE << " -- 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& /*unused*/)
    {
        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& /*unused*/)
    {
        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& /*unused*/)
    {
        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& /*unused*/)
    {
        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& /*unused*/)
    {
        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& /*unused*/)
    {
        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.empty())
        {
            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_VERBOSE << "--------------------------------------- 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_VERBOSE << "--------------------------------------- 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_VERBOSE << "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& /*unused*/, 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();
        }
        ARMARX_ERROR << mpName << " is not in the MP pool. Please check your configuration file";
        return 1.0;
    }
 
    bool
    MPPool::getMPEnabled(const Ice::Current& /*unused*/)
    {
        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_VERBOSE << "-- successfully updated MP config.";
        return true;
    }
 
    bool
    MPPool::resetMPsV1(const ::armarx::aron::data::dto::DictPtr& dto,
                       const std::map<std::string, VirtualRobot::RobotNodeSetPtr>& rnsMap,
                       const std::shared_ptr<common::coordination::Coordination>& coordinator)
    {
        const 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);
        }
 
        if (coordinator)
        {
            if (not coordinator->isInitialized())
            {
                usleep(1000);
                ARMARX_WARNING << "Coordinator is not yet initialized.";
                return false;
            }
            for (const auto& nodeSetName : coordinator->getNodesetList())
            {
                if (rnsMap.find(nodeSetName) == rnsMap.end())
                {
                    ARMARX_WARNING
                        << "Requested RobotNodeSet " << nodeSetName
                        << " by coordinator does not exist in the controller. MP won't be created!";
                    return false;
                }
            }
        }
        else
        {
            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);
 
        reInitMPInputOutputDataV1(rnsMap, coordinator);
        ARMARX_VERBOSE << "-- successfully updated MP config.";
        return true;
    }
 
    bool
    MPPool::resetMPsV2(const ::armarx::aron::data::dto::DictPtr& dto,
                       const std::map<std::string, MPReInitData>& data)
    {
        const 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 (data.find(mp.nodeSetName) == data.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);
 
        reInitMPInputOutputDataV2(data);
        ARMARX_VERBOSE << "-- successfully updated MP config.";
        return true;
    }
 
    ::armarx::aron::data::dto::DictPtr
    MPPool::getMPConfig(const Ice::Current& /*iceCurrent*/)
    {
        return mpConfig.toAronDTO();
    }
 
    void
    MPPool::reInitMPInputOutputDataV2(const std::map<std::string, MPReInitData>& data)
    {
        ARMARX_VERBOSE << "-- 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);
            const auto& d = data.at(mp.second.mp->getNodeSetName());
            const auto& role = mp.second.mp->getRole();
            if (role == "taskspace" or role == "object")
            {
                ARMARX_CHECK_EQUAL(d.pos.size(), 7)
                    << "the task space pos must have 7 dimensions, got " << d.pos.size();
                // ARMARX_CHECK_EQUAL(d.vel.size(), 6)
                //             << "the task space velocity must have 6 dimensions, got " << d.vel.size();
                const Eigen::Matrix4f currentPose = common::vec7fToMat4f(d.pos);
                ARMARX_VERBOSE << "---- (" << 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 (role == "nullspace" or role == "hand")
            {
                ARMARX_VERBOSE << "---- (" << index << ") init input output data for "
                               << mp.second.mp->getMPName() << " with " << d.pos.size()
                               << " joints";
                std::dynamic_pointer_cast<mp::JSMPInput>(mp.second.input)->angleRadian = d.pos;
                std::dynamic_pointer_cast<mp::JSMPInput>(mp.second.input)->angularVel = d.vel;
                std::dynamic_pointer_cast<mp::JSMPOutput>(mp.second.output)->angleRadian = d.pos;
                std::dynamic_pointer_cast<mp::JSMPOutput>(mp.second.output)->angularVel = d.vel;
            }
            // else if (_mp.second.mp->getRole() == "hand")
            // {
            //     ARMARX_VERBOSE << "---- (" << index << ") init input output data for "
            //                 << _mp.second.mp->getMPName() << " with "
            //                 << d.pos.size() << " joints";
            //     std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angleRadian = d.pos;
            //     std::dynamic_pointer_cast<mp::JSMPInput>(_mp.second.input)->angularVel = d.vel;
            //     std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angleRadian = d.pos;
            //     std::dynamic_pointer_cast<mp::JSMPOutput>(_mp.second.output)->angularVel = d.vel;
            // }
            index++;
        }
    }
 
    void
    MPPool::reInitMPInputOutputData(
        const std::map<std::string, VirtualRobot::RobotNodeSetPtr>& rnsMap)
    {
        ARMARX_VERBOSE << "-- 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);
            const VirtualRobot::RobotNodeSetPtr& rns = rnsMap.at(mp.second.mp->getNodeSetName());
 
            if (mp.second.mp->getRole() == "taskspace")
            {
                Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
                ARMARX_VERBOSE << "---- (" << 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_VERBOSE << "---- (" << 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_VERBOSE << "---- (" << 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++;
        }
    }
 
    void
    MPPool::reInitMPInputOutputDataV1(
        const std::map<std::string, VirtualRobot::RobotNodeSetPtr>& rnsMap,
        const std::shared_ptr<common::coordination::Coordination>& coordinator)
    {
        ARMARX_VERBOSE << "-- reconfigure MP: reinitialize the mp input output, as well as the rt "
                          "related buffer values";
        int index = 0;
        for (auto& mp : mps)
        {
            const auto& role = mp.second.mp->getRole();
            //            const auto& nodeSetName = ;
            //            ARMARX_CHECK_EQUAL(nodeSetName, limb.at(nodeSetName)->kinematicChainName);
 
            if (role == "taskspace")
            {
                const VirtualRobot::RobotNodeSetPtr& rns =
                    rnsMap.at(mp.second.mp->getNodeSetName());
                const Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
                ARMARX_VERBOSE << "---- (" << 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 (role == "object")
            {
                ARMARX_CHECK_NOT_NULL(coordinator)
                    << "When MP role is object, the coordinator must be active";
                ARMARX_CHECK(coordinator->isInitialized())
                    << "The coordinator must first be initialized.";
 
                const auto& coord =
                    std::dynamic_pointer_cast<common::coordination::SyncCoordination>(coordinator);
                const std::scoped_lock lock(coord->mtx_data_non_rt);
                const Eigen::Matrix4f currentPose = coord->dataNonRt.targetPose;
 
                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 (role == "nullspace" or role == "hand")
            {
                const VirtualRobot::RobotNodeSetPtr& rns =
                    rnsMap.at(mp.second.mp->getNodeSetName());
                ARMARX_VERBOSE << "---- (" << 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
            {
                ARMARX_WARNING << "MP of role " << role << " is not supported!";
            }
            index++;
        }
    }
 
} // namespace armarx::control::common::mp