JSMP.cpp

Go to the documentation of this file.

#include "JSMP.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
{
 
    JSMP::JSMP(const arondto::MPConfig& c) : MP{c}
    {
        ARMARX_INFO << "-- created JSMP 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
    JSMP::run(MPInputPtr input, MPOutputPtr output)
    {
        JSMPInputPtr in = std::dynamic_pointer_cast<JSMPInput>(input);
        JSMPOutputPtr out = std::dynamic_pointer_cast<JSMPOutput>(output);
        ARMARX_CHECK_NOT_NULL(in) << "\nInput pointer for JSMP has wrong type or empty";
        ARMARX_CHECK_NOT_NULL(out) << "\nOutput pointer for JSMP has wrong type or empty";
 
        out->angularVel.setZero(in->angularVel.size());
        if (running.load() and !paused.load())
        {
            //        ARMARX_INFO << VAROUT(canonicalValue);
            double phaseStop = 0;
            double tau = 1.0;
 
            if (canonicalValue < 0.01 && cfg.mpStyle == "Periodic")
            {
                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->angleRadian.size(), in->angularVel.size());
            for (int i = 0; i < in->angleRadian.size(); i++)
            {
                currentState[i].pos = in->angleRadian(i);
                currentState[i].vel = in->angularVel(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->angleRadian.size(), out->angularVel.size());
            for (int i = 0; i < in->angleRadian.size(); i++)
            {
                out->angleRadian(i) = targetState[i].pos;
                out->angularVel(i) = targetState[i].vel;
                //            ARMARX_INFO << VAROUT(targetState[i].pos) << VAROUT(targetState[i].vel);
            }
        }
    }
 
} // namespace armarx::control::common::mp