KeypointsMP.cpp

Go to the documentation of this file.

#include "KeypointsMP.h"
 
#include <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include "../utils.h"
 
namespace armarx::control::common::mp
{
 
    KeypointsMP::KeypointsMP(const MPConfig& c) : MP{c}
 
    {
        ARMARX_IMPORTANT << "created KeypointMP with name: " << VAROUT(cfg.name);
        //    if (cfg.viaPoints.size() > 0)
        //    {
        //        for (const auto& vp : cfg.viaPoints)
        //        {
        //            ARMARX_IMPORTANT << VAROUT(vp.canonicalValue) << " " << VAROUT(common::dVecToString(vp.viaPointValue)) << " " << VAROUT(vmp->getUmin());
        //            ARMARX_CHECK_GREATER_EQUAL(vp.canonicalValue, 0);
        //            ARMARX_CHECK_LESS_EQUAL(vp.canonicalValue, 1);
        //            userDefinedViaPoints.push_back({vp.canonicalValue, vp.viaPointValue});
        //        }
        //    }
    }
 
    void
    KeypointsMP::run(MPInputPtr input, MPOutputPtr output, const PhaseStopResult&)
    {
        if (not running.load())
        {
            return;
        }
        KeypointsMPInputPtr in = std::dynamic_pointer_cast<KeypointsMPInput>(input);
        KeypointsMPOutputPtr out = std::dynamic_pointer_cast<KeypointsMPOutput>(output);
        ARMARX_CHECK_NOT_NULL(in) << "\nInput pointer for KeypointsMP has wrong type or empty";
        ARMARX_CHECK_NOT_NULL(out) << "\nOutput pointer for KeypointsMP has wrong type or empty";
 
        out->keypointVelocity.setZero(in->keypointPosition.size());
        if (paused.load())
        {
            return;
        }
        //        ARMARX_INFO << VAROUT(canonicalValue) << VAROUT(out->keypointPosition) << VAROUT(out->keypointVelocity)
        //                    << VAROUT(in->keypointPosition) << VAROUT(in->keypointVelocity);
        double phaseStop = 0;
        double tau = 1.0;
 
        if (canonicalValue < 0.01 && cfg.mpStyle == "Periodic")
        {
            ARMARX_IMPORTANT << " -- Periodic mode: new iteration.";
            canonicalValue = 1.0;
        }
        if (canonicalValue < 1e-8 && cfg.mpStyle == "Discrete")
        {
            ARMARX_INFO << " -- Discrete mode: MP " << cfg.name << " finished.";
            running.store(false);
            return;
        }
 
        LockGuardType guard(mpMutex);
        ARMARX_CHECK_EQUAL(in->keypointPosition.size(), in->keypointVelocity.size());
        for (int i = 0; i < in->keypointPosition.size(); i++)
        {
            currentState[i].pos = in->keypointPosition(i);
            currentState[i].vel = in->keypointVelocity(i);
        }
        double deltaT = in->deltaT;
 
        double timeDuration = cfg.durationSec;
        canonicalValue -= tau * deltaT / ((1 + phaseStop) * timeDuration);
 
        std::vector<mplib::representation::MPState> targetState =
            std::dynamic_pointer_cast<mplib::representation::vmp::PrincipalComponentVMP>(vmp)
                ->calculateDesiredState(canonicalValue, currentState);
 
        ARMARX_CHECK_EQUAL(out->keypointPosition.size(), out->keypointVelocity.size());
        for (int i = 0; i < in->keypointPosition.size(); i++)
        {
            out->keypointPosition(i) = targetState[i].pos;
            out->keypointVelocity(i) = targetState[i].vel;
            //            ARMARX_INFO << VAROUT(targetState[i].pos) << VAROUT(targetState[i].vel);
        }
        if (firstRun.load())
        {
            firstRun.store(false);
        }
    }
 
} // namespace armarx::control::common::mp