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, const PhaseStopResult& phase)
    {
        if (not running.load())
        {
            return;
        }
        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 (paused.load())
        {
            return;
        }
 
        //        ARMARX_INFO << VAROUT(canonicalValue);
        double phaseStop = phase.phaseStop;
        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);
        }
        if (firstRun.load())
        {
            firstRun.store(false);
        }
    }
 
} // namespace armarx::control::common::mp