TSMP.cpp

Go to the documentation of this file.

#include "TSMP.h"
 
#include <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Clock.h>
 
#include "../utils.h"
 
namespace armarx::control::common::mp
{
 
    TSMP::TSMP(const MPConfig& c) : MP{c}
    {
 
        ARMARX_INFO << "    -- created TSMP with name: " << VAROUT(cfg.name);
        //    if (cfg.viaPoints.size() > 0)
        //    {
        //        for (const auto& vp : cfg.viaPoints)
        //        {
        ////            std::map<std::string, FramedPose> viapointsMapTemp;
        ////            viapointsMapTemp = userConfig["via_points"].get<decltype(viapointsMapTemp)>();
        //            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);
        //            PosePtr p = new Pose(vp.viaPointValue);
        //            userDefinedViaPoints.push_back({vp.canonicalValue, common::poseToDVec(p)});
        //        }
        //    }
    }
 
    void
    TSMP::validateInitialState(const armarx::control::common::mp::DVec& starts)
    {
        setStart(starts);
        vmp->setInitialState(currentState);
        vmp->prepareExecution();
    }
 
    DVec
    TSMP::validateViaPoint(const DVec& viapoint)
    {
        auto viaPoint = viapoint;
        if (prevViapoint.size() != viapoint.size())
        {
            prevViapoint = viapoint;
        }
        common::validateTSViaPoint(viaPoint, prevViapoint);
        return viaPoint;
    }
 
    void
    TSMP::run(MPInputPtr input, MPOutputPtr output)
    {
        if (not running.load())
        {
            return;
        }
        if (firstRun.load())
        {
            mpStartTimeSec = armarx::DateTime::Now();
        }
        TSMPInputPtr in = std::dynamic_pointer_cast<TSMPInput>(input);
        TSMPOutputPtr out = std::dynamic_pointer_cast<TSMPOutput>(output);
        ARMARX_CHECK_NOT_NULL(in) << "\nInput pointer for TSMP has wrong type or empty";
        ARMARX_CHECK_NOT_NULL(out) << "\nOutput pointer for TSMP has wrong type or empty";
 
        out->vel.setZero();
        if (paused.load())
        {
            return;
        }
 
        //        ARMARX_INFO << VAROUT(canonicalValue); // << VAROUT(deltaT) << VAROUT(currentPose);
        double phaseStop = 0.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")
        {
            auto duration = (armarx::DateTime::Now() - mpStartTimeSec).toSecondsDouble();
            ARMARX_INFO << " -- Discrete mode: MP " << cfg.name << " finished in " << duration
                        << " sec.";
            running.store(false);
            return;
        }
 
        LockGuardType guard(mpMutex);
        Eigen::Matrix4f currentPose = in->pose;
        // Eigen::Vector6f currentVel = in->vel;
 
        std::vector<double> currentPoseVec = common::mat4ToDVec(currentPose);
        for (size_t i = 0; i < currentState.size(); i++)
        {
            currentState[i].pos = currentPoseVec[i];
        }
        double deltaT = in->deltaT;
 
        //        /// calculate phase stop based on current pose and target pose
        //        //        if (cfg.enablePhaseStop)
        //        //        {
        //        double posiError = 0;
        //        double oriError = 0;
 
        //        Eigen::Vector3d targetPosition {targetPoseVec[0], targetPoseVec[1], targetPoseVec[2]};
        //        Eigen::Quaterniond targetQuaternion {targetPoseVec[3], targetPoseVec[4], targetPoseVec[5], targetPoseVec[6]}; // w,x,y,z convention
 
        //        getError(currentPose, targetPosition, targetQuaternion, posiError, oriError);
 
        //        double poseError = posiError + cfg.psMM2Radian * oriError;
 
        //        double phaseDist;
        //        if (isDisturbance)
        //        {
        //            phaseDist = cfg.backDist;
        //        }
        //        else
        //        {
        //            phaseDist = cfg.goDist;
        //        }
        //        double phaseL = cfg.maxValue;
        //        double phaseK = cfg.slop;
 
        //        phaseStop = phaseL / (1 + exp(-phaseK * (poseError - phaseDist)));
        //        double mpcFactor = 1 - (phaseStop / phaseL);
 
        //        if (mpcFactor < 0.1)
        //        {
        //            isDisturbance = true;
        //        }
 
        //        if (mpcFactor > 0.9)
        //        {
        //            isDisturbance = false;
        //        }
 
        double timeDuration = cfg.durationSec;
        canonicalValue -= tau * deltaT / ((1 + phaseStop) * timeDuration);
 
        std::vector<mplib::representation::MPState> targetState =
            std::dynamic_pointer_cast<mplib::representation::vmp::TaskSpacePrincipalComponentVMP>(
                vmp)
                ->calculateDesiredState(canonicalValue, currentState);
 
        // scale translation velocity
        for (size_t i = 0; i < 3; ++i)
        {
            currentState[i].vel = tau * targetState[i].vel * cfg.amplitude / timeDuration;
            currentState[i].pos += deltaT * currentState[i].vel;
        }
 
        //        // define the translation velocity
        //        if (cfg.enablePhaseStop)
        //        {
        //            float vel0, vel1;
 
        //            for (size_t i = 0; i < 3; i++)
        //            {
        //                vel0 = currentState[i].vel;
        //                vel1 = cfg.psKpPos * (targetPoseVec[i] - currentPose(i, 3)) - cfg.psKdPos * currentVel(i);
        //                targetVel(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
        //            }
        //        }
        //        else
        //        {
        for (size_t i = 0; i < 3; i++)
        {
            out->vel(i) = currentState[i].vel;
        }
        //        }
 
        // define the rotation velocity
        Eigen::Quaterniond dmpQuaternionVel{
            targetState[3].vel, targetState[4].vel, targetState[5].vel, targetState[6].vel};
        Eigen::Quaterniond dmpQuaternionPosi{
            currentState[3].pos, currentState[4].pos, currentState[5].pos, currentState[6].pos};
        Eigen::Quaterniond angularVel0 = dmpQuaternionVel * dmpQuaternionPosi.inverse();
        angularVel0.w() *= 2;
        angularVel0.x() *= 2;
        angularVel0.y() *= 2;
        angularVel0.z() *= 2;
 
 
        double angularChange = angularVel0.angularDistance(previousAngularVelocity);
        if (angularVel0.w() * previousAngularVelocity.w() < 0 &&
            angularVel0.x() * previousAngularVelocity.x() < 0 &&
            angularVel0.y() * previousAngularVelocity.y() < 0 &&
            angularVel0.z() * previousAngularVelocity.z() < 0 && angularChange < 1e-2)
        {
            angularVel0.w() = -angularVel0.w();
            angularVel0.x() = -angularVel0.x();
            angularVel0.y() = -angularVel0.y();
            angularVel0.z() = -angularVel0.z();
        }
        previousAngularVelocity = angularVel0;
 
        // scale orientation velocity
        angularVel0.w() = 0;
        angularVel0.x() =
            tau * angularVel0.x() * /*config.oriAmplitude*/ cfg.amplitude / timeDuration;
        angularVel0.y() =
            tau * angularVel0.y() * /*config.oriAmplitude*/ cfg.amplitude / timeDuration;
        angularVel0.z() =
            tau * angularVel0.z() * /*config.oriAmplitude*/ cfg.amplitude / timeDuration;
 
        //    Eigen::Quaterniond scaledQuat = (angularVel0 * dmpQuaternionPosi);
        //    currentState[3].vel = 0.5 * scaledQuat.w();
        //    currentState[4].vel = 0.5 * scaledQuat.x();
        //    currentState[6].vel = 0.5 * scaledQuat.z();
        //    currentState[5].vel = 0.5 * scaledQuat.y();
 
        for (size_t i = 3; i < 7; ++i)
        {
            currentState[i].vel =
                tau * targetState[i].vel * /*config.oriAmplitude*/ cfg.amplitude / timeDuration;
            currentState[i].pos += currentState[i].vel * deltaT;
        }
 
        //        if (cfg.enablePhaseStop)
        //        {
        //            Eigen::Quaternionf targetQuaternionf {(float)targetPoseVec[3], (float)targetPoseVec[4], (float)targetPoseVec[5], (float)targetPoseVec[6]};
 
        //            Eigen::Matrix3f desiredMat(targetQuaternionf);
        //            Eigen::Matrix3f currentMat = currentPose.block<3, 3>(0, 0);
        //            Eigen::Matrix3f diffMat = desiredMat * currentMat.inverse();
        //            Eigen::Vector3f diffRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
        //            Eigen::Vector3f angularVel1 = /*config.phaseStopParas.Kori*/cfg.psKpOri * diffRPY - /*config.phaseStopParas.Dori*/cfg.psKdOri * currentVel.tail<3>();
 
        //            targetVel(3) = mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1(0);
        //            targetVel(4) = mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1(1);
        //            targetVel(5) = mpcFactor * angularVel0.z() / timeDuration + (1 - mpcFactor) * angularVel1(2);
        //        }
        //        else
        //        {
        out->vel(3) = angularVel0.x();
        out->vel(4) = angularVel0.y();
        out->vel(5) = angularVel0.z();
        //        }
 
        //        //        debugData.canonicalValue = canonicalValue;
        //        //        debugData.oriError = oriError;
        //        //        debugData.posiError = posiError;
        //        //        debugData.mpcFactor = mpcFactor;
        //        //        debugData.poseError = poseError;
        //        Eigen::VectorXf desiredPosition;
        //        desiredPosition.setZero(currentState.size());
        //        for (size_t i = 0; i < currentState.size(); i++)
        //        {
        //            desiredPosition[i] = currentState[i][0];
        //        }
        std::vector<double> targetVec;
        for (const auto& t : targetState)
        {
            targetVec.push_back(t.pos);
        };
        out->pose = common::dVecToMat4(targetVec);
        if (firstRun.load())
        {
            firstRun.store(false);
        }
    }
 
} // namespace armarx::control::common::mp