DeprecatedNJointPeriodicTSDMPCompliantController.cpp

Go to the documentation of this file.

#include "DeprecatedNJointPeriodicTSDMPCompliantController.h"
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
 
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
    NJointControllerRegistration<DeprecatedNJointPeriodicTSDMPCompliantController> registrationControllerDeprecatedNJointPeriodicTSDMPCompliantController("DeprecatedNJointPeriodicTSDMPCompliantController");
 
    DeprecatedNJointPeriodicTSDMPCompliantController::DeprecatedNJointPeriodicTSDMPCompliantController(
        const RobotUnitPtr& robUnit,
        const armarx::NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        ARMARX_INFO << "creating periodic task-space impedance dmp controller";
 
        useSynchronizedRtRobot();
        cfg =  DeprecatedNJointPeriodicTSDMPCompliantControllerConfigPtr::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
 
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
        for (size_t i = 0; i < rns->getSize(); ++i)
        {
            std::string jointName = rns->getNode(i)->getName();
 
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
 
            const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No velocitySensor available for " << jointName;
            }
            if (!positionSensor)
            {
                ARMARX_WARNING << "No positionSensor available for " << jointName;
            }
 
            velocitySensors.push_back(velocitySensor);
            positionSensors.push_back(positionSensor);
        };
 
        tcp = rns->getTCP();
        ik.reset(new VirtualRobot::DifferentialIK(rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
        ik->setDampedSvdLambda(0.0001);
        numOfJoints = targets.size();
        qvel_filtered.setZero(targets.size());
        torqueLimit = cfg->jointTorqueLimit;
 
        /// setup force sensor and filters
        const SensorValueBase* svlf = robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
        forceSensor = svlf->asA<SensorValueForceTorque>();
 
        float tcpMass = 0.0f;
        Eigen::Vector3f tcpCoMInForceSensorFrame;
        tcpCoMInForceSensorFrame.setZero();
 
        enableTCPGravityCompensation.store(cfg->enableTCPGravityCompensation);
        ARMARX_INFO << VAROUT(enableTCPGravityCompensation);
        if (enableTCPGravityCompensation.load())
        {
            tcpMass = cfg->tcpMass;
            tcpCoMInForceSensorFrame = cfg->tcpCoMInForceSensorFrame;
        }
 
        forceOffset.setZero();
        torqueOffset.setZero();
        filteredForce.setZero();
        filteredTorque.setZero();
        filteredForceInRoot.setZero();
        filteredTorqueInRoot.setZero();
 
        /// Setup tool
        toolFrameInRoot = cfg->toolFrameInRoot;
        forcePID.reset(new PIDController(cfg->pidForce[0], cfg->pidForce[1], cfg->pidForce[2], cfg->pidForce[3]));
 
        /// setup DMP
        tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
        taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
        taskSpaceDMPConfig.DMPMode = "Linear";
        taskSpaceDMPConfig.DMPStyle = "Periodic";
        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
 
        dmpCtrl.reset(new tsvmp::TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
        kinematic_chain = cfg->nodeSetName;
 
        //        /// - null space
        //        useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
        //        nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
        //        isNullSpaceJointDMPLearned = false;
        //        ARMARX_CHECK_EQUAL(cfg->targetNullSpaceJointValues.size(), targets.size());
 
        /// initialize control parameters and buffers
        /// - control parameter
        ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
        ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
        CtrlParams initParams =
        {
            cfg->kpImpedance, cfg->kdImpedance,
            cfg->Knull, cfg->Dnull,
            cfg->pidForce,
            tcpMass, tcpCoMInForceSensorFrame
        };
        ctrlParams.reinitAllBuffers(initParams);
 
 
        ARMARX_CHECK_EQUAL(cfg->ws_x.size(), 2);
        ARMARX_CHECK_EQUAL(cfg->ws_y.size(), 2);
        ARMARX_CHECK_EQUAL(cfg->ws_z.size(), 2);
 
        // only for ARMAR-6 (safe-guard)
        if (!cfg->ignoreWSLimitChecks)
        {
            ARMARX_CHECK_LESS(cfg->ws_x[0], cfg->ws_x[1]);
            ARMARX_CHECK_LESS(cfg->ws_x[0], 1000);
            ARMARX_CHECK_LESS(-200, cfg->ws_x[1]);
 
            ARMARX_CHECK_LESS(cfg->ws_y[0],  cfg->ws_y[1]);
            ARMARX_CHECK_LESS(cfg->ws_y[0], 1200);
            ARMARX_CHECK_LESS(0,  cfg->ws_y[1]);
 
            ARMARX_CHECK_LESS(cfg->ws_z[0], cfg->ws_z[1]);
            ARMARX_CHECK_LESS(cfg->ws_z[0], 1800);
            ARMARX_CHECK_LESS(300, cfg->ws_z[1]);
        }
 
        changeTimer = 0;
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::onInitNJointController()
    {
        ARMARX_INFO << "init ...";
        /// initialize control parameters and buffers
        /// - control target
        DeprecatedNJointPeriodicTSDMPCompliantControllerControlData initData;
        initData.targetTSVel = Eigen::Vector6f::Zero();
        initData.targetForce = 0.0f;
        initData.targetNullSpaceJointValues = cfg->targetNullSpaceJointValues;
        reinitTripleBuffer(initData);
 
        /// - rt to interface
        RT2InterfaceData rt2InterfaceData;
        rt2InterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
        rt2InterfaceData.waitTimeForCalibration = 0;
        rt2InterfaceBuffer.reinitAllBuffers(rt2InterfaceData);
 
        /// - rt to mp
        RT2MPData rt2MPData;
        rt2MPData.deltaT = 0;
        rt2MPData.currentTime = 0;
        rt2MPData.currentPose = tcp->getPoseInRootFrame();
        rt2MPData.currentTwist.setZero();
        rt2MPData.isPhaseStop = false;
        rt2MPBuffer.reinitAllBuffers(rt2MPData);
 
 
        //        ARMARX_IMPORTANT << "read force sensor ...";
        //        forceOffset = forceSensor->force;
        //        ARMARX_IMPORTANT << "force offset: " << forceOffset;
        //        started = false;
 
        /// start mp thread
        runTask("DeprecatedNJointPeriodicTSDMPCompliantController", [&]
        {
            CycleUtil c(1);
            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
            while (getState() == eManagedIceObjectStarted)
            {
                if (isControllerActive())
                {
                    controllerRun();
                }
                c.waitForCycleDuration();
            }
        });
 
        ARMARX_IMPORTANT << "started controller ";
    }
 
    std::string DeprecatedNJointPeriodicTSDMPCompliantController::getClassName(const Ice::Current&) const
    {
        return "DeprecatedNJointPeriodicTSDMPCompliantController";
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::controllerRun()
    {
        if (!dmpCtrl || !rt2MPBuffer.updateReadBuffer())
        {
            return;
        }
 
        Eigen::Vector6f targetVels = Eigen::Vector6f::Zero();
        bool isPhaseStop = rt2MPBuffer.getUpToDateReadBuffer().isPhaseStop;
        if (!isPhaseStop && mpRunning.load())
        {
            double deltaT = rt2MPBuffer.getUpToDateReadBuffer().deltaT;
            Eigen::Matrix4f currentPose = rt2MPBuffer.getUpToDateReadBuffer().currentPose;
            Eigen::VectorXf currentTwist = rt2MPBuffer.getUpToDateReadBuffer().currentTwist;
 
 
            LockGuardType guardMP {mpMutex};
            dmpCtrl->flow(deltaT, currentPose, currentTwist);
 
            targetVels = dmpCtrl->getTargetVelocity();
            {
                debugMPBuffer.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
                debugMPBuffer.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
                debugMPBuffer.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
                VirtualRobot::MathTools::Quaternion currentQ = VirtualRobot::MathTools::eigen4f2quat(currentPose);
                debugMPBuffer.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
                debugMPBuffer.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
                debugMPBuffer.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
                debugMPBuffer.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
                debugMPBuffer.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
                debugMPBuffer.getWriteBuffer().mpcFactor =  dmpCtrl->debugData.mpcFactor;
                debugMPBuffer.getWriteBuffer().error = dmpCtrl->debugData.poseError;
                debugMPBuffer.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
                debugMPBuffer.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
                debugMPBuffer.getWriteBuffer().deltaT = deltaT;
            }
 
            LockGuardType guard {controlDataMutex};
            getWriterControlStruct().targetTSVel  = targetVels;
            getWriterControlStruct().targetTSPose = dmpCtrl->getTargetPoseMat();
            writeControlStruct();
        }
        else
        {
            LockGuardType guard{controlDataMutex};
            if (isPhaseStop)
            {
                getWriterControlStruct().targetTSPose = rt2MPBuffer.getUpToDateReadBuffer().currentPose;
            }
            getWriterControlStruct().targetTSVel = targetVels;
            writeControlStruct();
        }
 
        {
            debugMPBuffer.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
            debugMPBuffer.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
            debugMPBuffer.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
            debugMPBuffer.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
            debugMPBuffer.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
            debugMPBuffer.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
            debugMPBuffer.commitWrite();
        }
    }
 
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
    {
        /// ---------------------------- get current kinematics ---------------------------------------------
        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
 
        Eigen::VectorXf qpos(positionSensors.size());
        Eigen::VectorXf qvel(velocitySensors.size());
        for (size_t i = 0; i < positionSensors.size(); ++i)
        {
            qpos(i) = positionSensors[i]->position;
            qvel(i) = velocitySensors[i]->velocity;
        }
        qvel_filtered = (1 - cfg->qvelFilter) * qvel_filtered + cfg->qvelFilter * qvel;
        Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
        jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
 
        double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        /// ---------------------------- update control parameters ---------------------------------------------
        Eigen::Vector6f kpImpedance     = ctrlParams.getUpToDateReadBuffer().kpImpedance;
        Eigen::Vector6f kdImpedance     = ctrlParams.getUpToDateReadBuffer().kdImpedance;
        Eigen::VectorXf knull           = ctrlParams.getUpToDateReadBuffer().knull;
        Eigen::VectorXf dnull           = ctrlParams.getUpToDateReadBuffer().dnull;
        Eigen::Vector4f pidForce        = ctrlParams.getUpToDateReadBuffer().pidForce;
        forcePID->Kp                    = pidForce(0);
        forcePID->Ki                    = pidForce(1);
        forcePID->Kd                    = pidForce(2);
        forcePID->maxControlValue       = pidForce(3);
 
        float tcpMass                   = ctrlParams.getUpToDateReadBuffer().tcpMass;
        Eigen::Vector3f tcpCoMInFTFrame = ctrlParams.getUpToDateReadBuffer().tcpCoMInForceSensorFrame;
 
 
        /// ---------------------------- get current force torque value ---------------------------------------------
        Eigen::Vector3f forceBaseline   = Eigen::Vector3f::Zero();
        Eigen::Vector3f torqueBaseline  = Eigen::Vector3f::Zero();
        Eigen::Vector6f forceTorque;
        Eigen::Vector6f tcpGravityCompensation;
        forceTorque.setZero();
        tcpGravityCompensation.setZero();
 
        /// ---------------------------- get control targets ---------------------------------------------
        Eigen::Matrix4f targetPose;
        Eigen::Vector6f targetVel = Eigen::Vector6f::Zero();
        Eigen::VectorXf targetNullSpaceJointValues = rtGetControlStruct().targetNullSpaceJointValues;
        Eigen::Vector3f targetVelInTool = Eigen::Vector3f::Zero();
        Eigen::Matrix3f currentToolOriInRoot = Eigen::Matrix3f::Identity();
 
        float targetForce = 0.0f;
        if (rtFirstRun.load())
        {
            rtReady.store(false);
            rtFirstRun.store(false);
            timeForCalibration = 0;
            forceOffset.setZero();
            torqueOffset.setZero();
            forcePID->reset();
 
            Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
            forceFrameInTCP.block<3, 3>(0, 0) = currentPose.block<3, 3>(0, 0).transpose() * forceFrameInRoot.block<3, 3>(0, 0);
            tcpCoMInTCPFrame = forceFrameInTCP.block<3, 3>(0, 0) * tcpCoMInFTFrame;
 
            targetPose = currentPose;
            previousTargetPose = currentPose;
            toolOriInHand = currentPose.block<3, 3>(0, 0).transpose() * toolFrameInRoot.block<3, 3>(0, 0);
 
            ARMARX_IMPORTANT << "impedance control first run with " << VAROUT(targetPose);
 
            LockGuardType guard{controlDataMutex};
            getWriterControlStruct().targetTSPose = currentPose;
            writeControlStruct();
        }
        else
        {
            if (enableTCPGravityCompensation.load())
            {
                tcpGravityCompensation = getTCPGravityCompensation(currentPose, tcpMass);
            }
            if (!rtReady.load())
            {
                targetPose = previousTargetPose;
                forceOffset  = (1 - cfg->forceFilter) * forceOffset  + cfg->forceFilter * (forceSensor->force  - tcpGravityCompensation.head(3));
                torqueOffset = (1 - cfg->forceFilter) * torqueOffset + cfg->forceFilter * (forceSensor->torque - tcpGravityCompensation.tail(3));
                timeForCalibration = timeForCalibration + deltaT;
                if (timeForCalibration > cfg->waitTimeForCalibration)
                {
                    /// rt is ready only if the FT sensor is calibrated, and the rt2interface buffer is updated (see above)
                    ARMARX_IMPORTANT << "FT calibration done, RT is ready! \n" << VAROUT(forceOffset) << "\n" << VAROUT(torqueOffset) << "\n" << VAROUT(targetPose);
                    rtReady.store(true);
                }
            }
            else
            {
                /// update tool information
                currentToolOriInRoot = currentPose.block<3, 3>(0, 0) * toolOriInHand;
                forceTorque = getFilteredForceTorque(forceBaseline, torqueBaseline, tcpGravityCompensation);
                Eigen::VectorXf ftInTool = forceTorque;
                ftInTool.head(3) = currentToolOriInRoot.transpose() * ftInTool.head(3);
 
                targetForce = rtGetControlStruct().targetForce;
                targetVel   = rtGetControlStruct().targetTSVel;
                targetPose  = rtGetControlStruct().targetTSPose;
 
                forcePID->update(deltaT, ftInTool(2), targetForce);
                float forcePIDVel = -1000.0f * (float)forcePID->getControlValue();  /// convert to mm/s as in ArmarX
 
                targetVelInTool(2) = forcePIDVel;
                targetVel.head(3) += currentToolOriInRoot * targetVelInTool;
 
 
                targetPose.block<3, 1>(0, 3) = targetPose.block<3, 1>(0, 3) + (float)deltaT * targetVel.block<3, 1>(0, 0);
                if ((previousTargetPose.block<3, 1>(0, 3) - targetPose.block<3, 1>(0, 3)).norm() > 100.0f)
                {
                    ARMARX_WARNING << "new target \n" << VAROUT(targetPose) << "\nis too far away from\n" << VAROUT(previousTargetPose);
                    targetPose = previousTargetPose;
                }
                previousTargetPose = targetPose;
            }
        }
 
        bool isPhaseStop = false;
        float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
        if (diff > cfg->phaseDist0)
        {
            isPhaseStop = true;
        }
 
        if (isPhaseStop)
        {
            Eigen::Vector2f currentXY = currentPose.block<2, 1>(0, 3);
            if ((lastPosition - currentXY).norm() < cfg->changePositionTolerance)
            {
                changeTimer += deltaT;
            }
            else
            {
                lastPosition = currentPose.block<2, 1>(0, 3);
                changeTimer = 0;
            }
 
            if (changeTimer > cfg->changeTimerThreshold)
            {
                targetPose(0, 3) = currentPose(0, 3);
                targetPose(1, 3) = currentPose(1, 3);
                isPhaseStop = false;
                changeTimer = 0;
            }
        }
        else
        {
            changeTimer = 0;
        }
 
        targetPose(0, 3) = std::clamp(targetPose(0, 3), cfg->ws_x[0], cfg->ws_x[1]);
        targetPose(1, 3) = std::clamp(targetPose(1, 3), cfg->ws_y[0], cfg->ws_y[1]);
        targetPose(2, 3) = std::clamp(targetPose(2, 3), cfg->ws_z[0], cfg->ws_z[1]);
 
        // write rt buffers
        {
            rt2InterfaceBuffer.getWriteBuffer().currentTcpPose = currentPose;
            rt2InterfaceBuffer.getWriteBuffer().waitTimeForCalibration += deltaT;
            rt2InterfaceBuffer.commitWrite();
 
            debugRTBuffer.getWriteBuffer().targetPose = targetPose;
            debugRTBuffer.getWriteBuffer().currentPose = currentPose;
            debugRTBuffer.getWriteBuffer().filteredForce = filteredForceInRoot;
            debugRTBuffer.getWriteBuffer().targetVel = targetVel;
            debugRTBuffer.getWriteBuffer().isPhaseStop = isPhaseStop;
            debugRTBuffer.getWriteBuffer().targetVelInTool = targetVelInTool;
 
            rt2MPBuffer.getWriteBuffer().currentPose = currentPose;
            rt2MPBuffer.getWriteBuffer().currentTwist = currentTwist;
            rt2MPBuffer.getWriteBuffer().deltaT = deltaT;
            rt2MPBuffer.getWriteBuffer().currentTime += deltaT;
            rt2MPBuffer.getWriteBuffer().isPhaseStop = isPhaseStop;
            rt2MPBuffer.commitWrite();
        }
 
        /// ----------------------------- Impedance control ---------------------------------------------
        /// calculate pose error between virtual pose and current pose
        /// !!! This is very important: you have to keep postion and orientation both
        /// with UI unit (meter, radian) to calculate impedance force.
        Eigen::Vector6f poseErrorImp;
        Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).transpose();
        poseErrorImp.head(3) = 0.001 * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3));
        currentTwist.head(3) *= 0.001;
        poseErrorImp.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
        Eigen::Vector6f forceImpedance = kpImpedance.cwiseProduct(poseErrorImp) - kdImpedance.cwiseProduct(currentTwist);
 
        /// ----------------------------- Nullspace PD Control --------------------------------------------------
        Eigen::VectorXf nullspaceTorque = knull.cwiseProduct(targetNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
 
        /// ----------------------------- Map TS target force to JS --------------------------------------------------
        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
        Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * forceImpedance + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
 
        /// ----------------------------- Send torque command to joint device --------------------------------------------------
        ARMARX_CHECK_EXPRESSION(!targets.empty());
        for (size_t i = 0; i < targets.size(); ++i)
        {
            targets.at(i)->torque = jointDesiredTorques(i);
            if (!targets.at(i)->isValid() || isnan(targets.at(i)->torque))
            {
                targets.at(i)->torque = 0.0f;
            }
            else
            {
                if (fabs(targets.at(i)->torque) > fabs(torqueLimit))
                {
                    targets.at(i)->torque = fabs(torqueLimit) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
                }
            }
        }
 
        debugRTBuffer.commitWrite();
    }
 
    Eigen::Vector6f DeprecatedNJointPeriodicTSDMPCompliantController::getTCPGravityCompensation(Eigen::Matrix4f& currentPose, float tcpMass)
    {
        // static compensation
        Eigen::Vector3f gravity;
        gravity << 0.0, 0.0, -9.8;
        Eigen::Vector3f localGravity = currentPose.block<3, 3>(0, 0).transpose() * gravity;
        Eigen::Vector3f localForceVec = tcpMass * localGravity;
        Eigen::Vector3f localTorqueVec = tcpCoMInTCPFrame.cross(localForceVec);
 
        Eigen::Vector6f tcpGravityCompensation;
        tcpGravityCompensation << localForceVec, localTorqueVec;
        return tcpGravityCompensation;
    }
 
    Eigen::Vector6f DeprecatedNJointPeriodicTSDMPCompliantController::getFilteredForceTorque(Eigen::Vector3f& forceBaseline, Eigen::Vector3f& torqueBaseline, Eigen::Vector6f& handCompensatedFT)
    {
        filteredForce  = (1 - cfg->forceFilter) * filteredForce  + cfg->forceFilter * forceSensor->force;
        filteredTorque = (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * forceSensor->torque;
 
        Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
        filteredForceInRoot  = forceFrameInRoot.block<3, 3>(0, 0) * (filteredForce  - forceOffset  - handCompensatedFT.head(3)) - forceBaseline;
        filteredTorqueInRoot = forceFrameInRoot.block<3, 3>(0, 0) * (filteredTorque - torqueOffset - handCompensatedFT.tail(3)) - torqueBaseline;
 
        for (size_t i = 0; i < 3; ++i)
        {
            if (fabs(filteredForceInRoot(i)) > cfg->forceDeadZone)
            {
                filteredForceInRoot(i) -= (filteredForceInRoot(i) / fabs(filteredForceInRoot(i))) * cfg->forceDeadZone;
            }
            else
            {
                filteredForceInRoot(i) = 0;
            }
 
            if (fabs(filteredTorqueInRoot(i)) > cfg->torqueDeadZone)
            {
                filteredTorqueInRoot(i) -= (filteredTorqueInRoot(i) / fabs(filteredTorqueInRoot(i))) * cfg->torqueDeadZone;
            }
            else
            {
                filteredTorqueInRoot(i) = 0;
            }
        }
 
        Eigen::Vector6f forceTorque;
        forceTorque << filteredForceInRoot, filteredTorqueInRoot;
        return forceTorque;
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::setKpImpedance(const Eigen::Vector6f& kpImpedance, const Ice::Current&)
    {
        LockGuardType guard(mpMutex);
        ctrlParams.getWriteBuffer().kpImpedance = kpImpedance;
        ctrlParams.commitWrite();
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::setKdImpedance(const Eigen::Vector6f& kdImpedance, const Ice::Current&)
    {
        LockGuardType guard(mpMutex);
        ctrlParams.getWriteBuffer().kdImpedance = kdImpedance;
        ctrlParams.commitWrite();
    }
 
    std::string DeprecatedNJointPeriodicTSDMPCompliantController::getKinematicChainName(const Ice::Current&)
    {
        return kinematic_chain;
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
    {
        ARMARX_INFO << "Learning DMP ... ";
 
        LockGuardType guard {mpMutex};
        dmpCtrl->learnDMPFromFiles(fileNames);
 
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::learnDMPFromTrajectory(const TrajectoryBasePtr& trajectory, const Ice::Current&)
    {
        ARMARX_INFO << "Learning DMP ... ";
        ARMARX_CHECK_EXPRESSION(trajectory);
        TrajectoryPtr dmpTraj = TrajectoryPtr::dynamicCast(trajectory);
        ARMARX_CHECK_EXPRESSION(dmpTraj);
 
        LockGuardType guard {mpMutex};
        dmpCtrl->learnDMPFromTrajectory(dmpTraj);
 
    }
 
    bool DeprecatedNJointPeriodicTSDMPCompliantController::isFinished(const Ice::Current&)
    {
        return false;
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::setSpeed(Ice::Double times, const Ice::Current&)
    {
        LockGuardType guard {mpMutex};
        dmpCtrl->setSpeed(times);
    }
 
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
    {
        LockGuardType guard {mpMutex};
        dmpCtrl->setGoalPoseVec(goals);
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::setAmplitude(Ice::Double amp, const Ice::Current&)
    {
        LockGuardType guard {mpMutex};
        dmpCtrl->setAmplitude(amp);
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
    {
        LockGuardType guard {controlDataMutex};
        getWriterControlStruct().targetForce = targetForce;
        writeControlStruct();
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::runDMP(const Ice::DoubleSeq&  goals, Ice::Double tau, const Ice::Current&)
    {
        rtFirstRun.store(true);
        while (rtFirstRun.load() || !rtReady.load() || !rt2InterfaceBuffer.updateReadBuffer())
        {
            usleep(100);
        }
 
        Eigen::Matrix4f pose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentTcpPose;
 
        LockGuardType guard {mpMutex};
        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
        dmpCtrl->setSpeed(tau);
 
        mpRunning.store(true);
        dmpCtrl->resumeController();
        ARMARX_INFO << "start mp ...";
    }
 
    double DeprecatedNJointPeriodicTSDMPCompliantController::getCanVal(const Ice::Current&)
    {
        return dmpCtrl->canVal;
    }
 
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::resetDMP(const Ice::Current&)
    {
        //        if (started)
        if (mpRunning.load())
        {
            ARMARX_INFO << "Cannot reset running DMP";
        }
        rtFirstRun = true;
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::stopDMP(const Ice::Current&)
    {
        mpRunning.store(false);
        //        oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
        //        started = false;
        //        stopped = true;
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::pauseDMP(const Ice::Current&)
    {
        dmpCtrl->pauseController();
    }
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::resumeDMP(const Ice::Current&)
    {
        dmpCtrl->resumeController();
        //        stopped = false;
    }
 
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
    {
        std::string datafieldName;
        std::string debugName = "Periodic";
        StringVariantBaseMap datafields;
 
        Eigen::Matrix4f targetPoseDebug = debugRTBuffer.getUpToDateReadBuffer().targetPose;
        datafields["target_x"] = new Variant(targetPoseDebug(0, 3));
        datafields["target_y"] = new Variant(targetPoseDebug(1, 3));
        datafields["target_z"] = new Variant(targetPoseDebug(2, 3));
 
        Eigen::Vector3f targetVelInTool = debugRTBuffer.getUpToDateReadBuffer().targetVelInTool;
        datafields["targetVelInTool_x"] = new Variant(targetVelInTool(0));
        datafields["targetVelInTool_y"] = new Variant(targetVelInTool(1));
        datafields["targetVelInTool_z"] = new Variant(targetVelInTool(2));
 
        Eigen::Matrix4f currentPoseDebug = debugRTBuffer.getUpToDateReadBuffer().currentPose;
        datafields["current_x"] = new Variant(currentPoseDebug(0, 3));
        datafields["current_y"] = new Variant(currentPoseDebug(1, 3));
        datafields["current_z"] = new Variant(currentPoseDebug(2, 3));
 
        Eigen::Vector3f filteredForce = debugRTBuffer.getUpToDateReadBuffer().filteredForce;
        datafields["filteredforce_x"] = new Variant(filteredForce(0));
        datafields["filteredforce_y"] = new Variant(filteredForce(1));
        datafields["filteredforce_z"] = new Variant(filteredForce(2));
 
 
        Eigen::Vector3f reactForce = debugRTBuffer.getUpToDateReadBuffer().reactForce;
        datafields["reactForce_x"] = new Variant(reactForce(0));
        datafields["reactForce_y"] = new Variant(reactForce(1));
        datafields["reactForce_z"] = new Variant(reactForce(2));
 
        Eigen::VectorXf targetVel = debugRTBuffer.getUpToDateReadBuffer().targetVel;
        datafields["targetVel_x"] = new Variant(targetVel(0));
        datafields["targetVel_y"] = new Variant(targetVel(1));
        datafields["targetVel_z"] = new Variant(targetVel(2));
 
        datafields["canVal"] = new Variant(debugMPBuffer.getUpToDateReadBuffer().currentCanVal);
        datafields["deltaT"] = new Variant(debugMPBuffer.getUpToDateReadBuffer().deltaT);
 
        datafields["PhaseStop"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().isPhaseStop);
 
 
        //        datafields["targetVel_rx"] = new Variant(targetVel(3));
        //        datafields["targetVel_ry"] = new Variant(targetVel(4));
        //        datafields["targetVel_rz"] = new Variant(targetVel(5));
 
        //        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
        //        for (auto& pair : values)
        //        {
        //            datafieldName = pair.first  + "_" + debugName;
        //            datafields[datafieldName] = new Variant(pair.second);
        //        }
 
        //        auto currentPose = debugOutputData.getUpToDateReadBuffer().currentPose;
        //        for (auto& pair : currentPose)
        //        {
        //            datafieldName = pair.first + "_" + debugName;
        //            datafields[datafieldName] = new Variant(pair.second);
        //        }
 
        //        datafieldName = "canVal_" + debugName;
        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
        //        datafieldName = "mpcFactor_" + debugName;
        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
        //        datafieldName = "error_" + debugName;
        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
        //        datafieldName = "posError_" + debugName;
        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
        //        datafieldName = "oriError_" + debugName;
        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
        //        datafieldName = "deltaT_" + debugName;
        //        datafields[datafieldName] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
 
        datafieldName = "PeriodicDMP";
        debugObs->setDebugChannel(datafieldName, datafields);
    }
 
 
 
    void DeprecatedNJointPeriodicTSDMPCompliantController::onDisconnectNJointController()
    {
        ARMARX_INFO << "stopped ...";
    }
 
 
 
}