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::computePhaseStop(const MPInputPtr input, const MPOutputPtr output, PhaseStopResult& phase)
    {
        if (!cfg.enablePhaseStop)
        {
            phase.phaseStop = 0.0;
            phase.mpcFactor = 1.0;
            return;
        }
 
        TSMPInputPtr in = std::dynamic_pointer_cast<TSMPInput>(input);
        TSMPOutputPtr out = std::dynamic_pointer_cast<TSMPOutput>(output);
 
        Eigen::Matrix4f currentPose = in->pose;
        Eigen::Vector6f currentVel = in->vel; /// Necessary for the D-term in Phase Stop
 
        auto targetPoseVec = common::mat4ToDVec(out->pose);
 
        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
        getError(currentPose, targetPosition, targetQuaternion, posiError, oriError);
 
        // Weighted Total Error
        double poseError = posiError + (cfg.psMM2Radian * oriError);
 
        // Hysteresis Threshold Selection
        double phaseDist = isDisturbance ? cfg.backDist : cfg.goDist;
 
        //        double phaseL = cfg.maxValue;
        //        double phaseK = cfg.slop;
 
        // Sigmoid Function
        phase.phaseStop = cfg.maxValue / (1 + exp(-cfg.slop * (poseError - phaseDist)));
 
        // Mixing Factor (1.0 = Trust MP, 0.0 = Trust Feedback)
        phase.mpcFactor = 1.0 - (phase.phaseStop / cfg.maxValue);
 
        // Update Disturbance State (Hysteresis)
        if (phase.mpcFactor < 0.1)
        {
            isDisturbance = true;
        }
        if (phase.mpcFactor > 0.9)
        {
            isDisturbance = false;
        }
    }
 
    void
    TSMP::run(MPInputPtr input, MPOutputPtr output, const PhaseStopResult& phase)
    {
        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;
        }
 
        double phaseStop = phase.phaseStop;
        double mpcFactor = phase.mpcFactor; // Default to pure MP execution (1.0)
        double tau = 1.0;
        double timeDuration = cfg.durationSec;
 
        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; /// Necessary for the D-term in Phase Stop
        double deltaT = in->deltaT;
 
        /// Sync internal state with current robot pose
        std::vector<double> currentPoseVec = common::mat4ToDVec(currentPose);
        for (size_t i = 0; i < currentState.size(); i++)
        {
            currentState[i].pos = currentPoseVec[i];
        }
 
        //        /// 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;
        //        }
 
        /// --------------------------------------------------------
        /// Time Evolution (Canonical Value) and Step MP forward
        /// --------------------------------------------------------
        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;
        }
 
        /// ----------------------------------------------------
        /// Velocity Blending (Translation)
        /// ----------------------------------------------------
        if (cfg.enablePhaseStop and cfg.enablePhaseStopMPC)
        {
            for (size_t i = 0; i < 3; i++)
            {
                const double vel0 = currentState[i].vel;
 
                /// PD Controller pulling towards target
                const double vel1 = (cfg.psKpPos * (targetPoseVec[i] - currentPose(i, 3))) -
                    (cfg.psKdPos * currentVel(i));
 
                /// Blend: Trust MP if mpcFactor is 1, Trust PD if mpcFactor is 0
                out->vel(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
            }
        }
        else
        {
            for (size_t i = 0; i < 3; i++)
            {
                out->vel(i) = currentState[i].vel;
            }
        }
 
        /// ----------------------------------------------------
        /// Velocity Blending (Orientation)
        /// ----------------------------------------------------
        /// Compute MP angular 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;
 
        /// Check for quaternion flip (continuity)
        double angularChange = angularVel0.angularDistance(previousAngularVelocity);
        if (angularVel0.coeffs().dot(previousAngularVelocity.coeffs()) < 0 && angularChange < 1e-2)
        {
            angularVel0.coeffs() = -angularVel0.coeffs();
        }
        //        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 by amplitude/duration
        angularVel0.w() = 0;
        angularVel0.vec() = tau * angularVel0.vec() * cfg.amplitude / timeDuration;
        //        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();
 
        /// Update internal state
        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 and cfg.enablePhaseStopMPC)
        {
            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);
 
            /// Orientation PD
            Eigen::Matrix3f diffMat = desiredMat * currentMat.inverse();
            Eigen::Vector3f diffRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
            Eigen::Vector3f angularVel1 =
                cfg.psKpOri * diffRPY - cfg.psKdOri * currentVel.tail<3>();
 
            /// Blend
            out->vel(3) =
                mpcFactor * angularVel0.x() / timeDuration + (1 - mpcFactor) * angularVel1(0);
            out->vel(4) =
                mpcFactor * angularVel0.y() / timeDuration + (1 - mpcFactor) * angularVel1(1);
            out->vel(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();
        }
 
        /// ----------------------------------------------------
        /// Output Pose
        /// ----------------------------------------------------
        /// Flatten targetState to vector and normalize quaternion
        std::vector<double> targetVec;
        for (const auto& t : targetState)
        {
            targetVec.push_back(t.pos);
        };
 
        auto start = targetVec.end() - 4;
 
        /// Compute norm of the last 4 elements
        double norm = std::sqrt(std::inner_product(start, targetVec.end(), start, 0.0));
 
        if (norm > 1e-8)
        {
            for (auto it = start; it != targetVec.end(); ++it)
            {
                *it /= norm;
            }
        }
 
        out->pose = common::dVecToMat4(targetVec);
 
        if (firstRun.load())
        {
            firstRun.store(false);
        }
    }
 
} // namespace armarx::control::common::mp