NJointAnomalyDetectionAdaptiveWipingController.cpp

Go to the documentation of this file.

#include "NJointAnomalyDetectionAdaptiveWipingController.h"
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
#include <RobotAPI/libraries/core/CartesianVelocityController.h>
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/core/Trajectory.h>
 
namespace armarx::control::deprecated_njoint_mp_controller::adaptive
{
    NJointControllerRegistration<NJointAnomalyDetectionAdaptiveWipingController>
        registrationControllerNJointAnomalyDetectionAdaptiveWipingController(
            "NJointAnomalyDetectionAdaptiveWipingController");
 
    NJointAnomalyDetectionAdaptiveWipingController::NJointAnomalyDetectionAdaptiveWipingController(
        const RobotUnitPtr& robUnit,
        const armarx::NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        useSynchronizedRtRobot();
        cfg = NJointAnomalyDetectionAdaptiveWipingControllerConfigPtr::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>();
 
            velocitySensors.push_back(velocitySensor);
            positionSensors.push_back(positionSensor);
        };
 
        useDMPInGlobalFrame = cfg->useDMPInGlobalFrame;
 
        tcp = rns->getTCP();
        // set tcp controller
        nodeSetName = cfg->nodeSetName;
        ik.reset(new VirtualRobot::DifferentialIK(
            rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
 
        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.Kori = cfg->phaseKpOri;
        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
 
        lastCanVal = cfg->timeDuration;
 
        dmpCtrl.reset(new tsvmp::TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
 
        NJointAnomalyDetectionAdaptiveWipingControllerControlData initData;
        initData.targetTSVel.resize(6);
        for (size_t i = 0; i < 6; ++i)
        {
            initData.targetTSVel(i) = 0;
        }
        initData.targetTSPose = tcp->getPoseInRootFrame();
        reinitTripleBuffer(initData);
 
        firstRun = true;
        dmpRunning = false;
 
        // anomaly detection
        velocityHorizon = cfg->velocityHorizon;
 
        // friction estimation
        frictionHorizon = cfg->frictionHorizon;
        estimatedFriction << 0.0, 0.0;
        lastForceInToolXY.setZero();
 
        ARMARX_CHECK_EQUAL(cfg->Kpos.size(), 3);
        ARMARX_CHECK_EQUAL(cfg->Dpos.size(), 3);
        ARMARX_CHECK_EQUAL(cfg->Kori.size(), 3);
        ARMARX_CHECK_EQUAL(cfg->Dori.size(), 3);
 
        kpos << cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2];
        dpos << cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2];
        kori << cfg->Kori[0], cfg->Kori[1], cfg->Kori[2];
        dori << cfg->Dori[0], cfg->Dori[1], cfg->Dori[2];
 
        isForceCtrlInForceDir = cfg->isForceCtrlInForceDir;
        isForceControlEnabled = cfg->isForceControlEnabled;
        isRotControlEnabled = cfg->isRotControlEnabled;
        isTorqueControlEnabled = cfg->isTorqueControlEnabled;
        isLCRControlEnabled = cfg->isLCRControlEnabled;
        forcePID.reset(new PIDController(
            cfg->pidForce[0], cfg->pidForce[1], cfg->pidForce[2], cfg->pidForce[3]));
        rotPID.reset(
            new PIDController(cfg->pidRot[0], cfg->pidRot[1], cfg->pidRot[2], cfg->pidRot[3]));
        torquePID.reset(new PIDController(
            cfg->pidTorque[0], cfg->pidTorque[1], cfg->pidTorque[2], cfg->pidTorque[3]));
        lcrPID.reset(
            new PIDController(cfg->pidLCR[0], cfg->pidLCR[1], cfg->pidLCR[2], cfg->pidLCR[3]));
        adaptKpForce = cfg->pidForce[0];
        adaptKpRot = cfg->pidRot[0];
 
        knull.setZero(targets.size());
        dnull.setZero(targets.size());
 
        for (size_t i = 0; i < targets.size(); ++i)
        {
            knull(i) = cfg->Knull.at(i);
            dnull(i) = cfg->Dnull.at(i);
        }
 
        nullSpaceJointsVec.resize(cfg->desiredNullSpaceJointValues.size());
        for (size_t i = 0; i < cfg->desiredNullSpaceJointValues.size(); ++i)
        {
            nullSpaceJointsVec(i) = cfg->desiredNullSpaceJointValues.at(i);
        }
 
 
        const SensorValueBase* svlf =
            robUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
        forceSensor = svlf->asA<SensorValueForceTorque>();
 
 
        ARMARX_CHECK_EQUAL(cfg->ftOffset.size(), 6);
 
        currentForceOffset.setZero();
        forceOffset << cfg->ftOffset[0], cfg->ftOffset[1], cfg->ftOffset[2];
        torqueOffset << cfg->ftOffset[3], cfg->ftOffset[4], cfg->ftOffset[5];
 
        handMass = cfg->handMass;
        handCOM << cfg->handCOM[0], cfg->handCOM[1], cfg->handCOM[2];
 
 
        filteredForce.setZero();
        filteredTorque.setZero();
        filteredFTCommand.setZero();
        filteredForceInRoot.setZero();
        filteredTorqueInRoot.setZero();
        targetFTInToolFrame.setZero();
 
        UserToRTData initUserData;
        initUserData.targetForce = 0;
        user2rtData.reinitAllBuffers(initUserData);
 
        oriToolDir << 0, 0, 1;
        gravityInRoot << 0, 0, -9.8;
 
        qvel_filtered.setZero(targets.size());
 
        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)
        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]);
 
        adaptK = kpos;
        adaptD = dpos;
        adaptKOri = kori;
        adaptDOri = dori;
        adaptKNull = knull;
        adaptDNull = dnull;
        lastDiff = 0;
        changeTimer = 0;
 
        abnormalFlag = false;
        lastAbnormalFlag = false;
        positionOffset.setZero();
 
        //        toolToFTSensorLink =
        //                rtGetRobot()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame().block<3,1>(0, 3) -
        //                tcp->getPoseInRootFrame().block<3,1>(0,3)
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::onInitNJointController()
    {
        ARMARX_INFO << "init ...";
 
 
        RTToControllerData initSensorData;
        initSensorData.deltaT = 0;
        initSensorData.currentTime = 0;
        initSensorData.currentPose = tcp->getPoseInRootFrame();
        initSensorData.currentTwist.setZero();
        initSensorData.isPhaseStop = false;
        rt2CtrlData.reinitAllBuffers(initSensorData);
 
        RTToUserData initInterfaceData;
        initInterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
        initInterfaceData.waitTimeForCalibration = 0;
        rt2UserData.reinitAllBuffers(initInterfaceData);
 
        started = false;
 
        runTask("NJointAnomalyDetectionAdaptiveWipingController",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            controllerRun();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    std::string
    NJointAnomalyDetectionAdaptiveWipingController::getClassName(const Ice::Current&) const
    {
        return "NJointAnomalyDetectionAdaptiveWipingController";
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::controllerRun()
    {
        if (!started)
        {
            return;
        }
 
        if (!dmpCtrl)
        {
            return;
        }
 
        Eigen::VectorXf targetVels(6);
        Eigen::Matrix4f targetDMPPose;
        bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
        if (isPhaseStop)
        {
            targetVels.setZero();
            targetDMPPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
        }
        else
        {
 
            double deltaT = rt2CtrlData.getUpToDateReadBuffer().deltaT;
            Eigen::Matrix4f currentPose = rt2CtrlData.getUpToDateReadBuffer().currentPose;
            Eigen::VectorXf currentTwist = rt2CtrlData.getUpToDateReadBuffer().currentTwist;
 
            LockGuardType guard{controllerMutex};
            dmpCtrl->flow(deltaT, currentPose, currentTwist);
 
            targetVels = dmpCtrl->getTargetVelocity();
            targetDMPPose = dmpCtrl->getTargetPoseMat();
 
            debugOutputData.getWriteBuffer().latestTargetVelocities["x_vel"] = targetVels(0);
            debugOutputData.getWriteBuffer().latestTargetVelocities["y_vel"] = targetVels(1);
            debugOutputData.getWriteBuffer().latestTargetVelocities["z_vel"] = targetVels(2);
            debugOutputData.getWriteBuffer().latestTargetVelocities["roll_vel"] = targetVels(3);
            debugOutputData.getWriteBuffer().latestTargetVelocities["pitch_vel"] = targetVels(4);
            debugOutputData.getWriteBuffer().latestTargetVelocities["yaw_vel"] = targetVels(5);
            debugOutputData.getWriteBuffer().currentPose["currentPose_x"] = currentPose(0, 3);
            debugOutputData.getWriteBuffer().currentPose["currentPose_y"] = currentPose(1, 3);
            debugOutputData.getWriteBuffer().currentPose["currentPose_z"] = currentPose(2, 3);
            VirtualRobot::MathTools::Quaternion currentQ =
                VirtualRobot::MathTools::eigen4f2quat(currentPose);
            debugOutputData.getWriteBuffer().currentPose["currentPose_qw"] = currentQ.w;
            debugOutputData.getWriteBuffer().currentPose["currentPose_qx"] = currentQ.x;
            debugOutputData.getWriteBuffer().currentPose["currentPose_qy"] = currentQ.y;
            debugOutputData.getWriteBuffer().currentPose["currentPose_qz"] = currentQ.z;
            debugOutputData.getWriteBuffer().currentCanVal = dmpCtrl->debugData.canVal;
            debugOutputData.getWriteBuffer().mpcFactor = dmpCtrl->debugData.mpcFactor;
            debugOutputData.getWriteBuffer().error = dmpCtrl->debugData.poseError;
            debugOutputData.getWriteBuffer().posError = dmpCtrl->debugData.posiError;
            debugOutputData.getWriteBuffer().oriError = dmpCtrl->debugData.oriError;
            debugOutputData.getWriteBuffer().deltaT = deltaT;
            debugOutputData.commitWrite();
        }
        getWriterControlStruct().canVal = dmpCtrl->canVal;
        getWriterControlStruct().targetTSVel = targetVels;
        getWriterControlStruct().targetTSPose = targetDMPPose;
        writeControlStruct();
 
        dmpRunning = true;
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::rtRun(
        const IceUtil::Time& sensorValuesTimestamp,
        const IceUtil::Time& timeSinceLastIteration)
    {
        double deltaT = timeSinceLastIteration.toSecondsDouble();
        float dTf = (float)deltaT;
 
        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
 
 
        Eigen::Vector3f currentToolDir;
        currentToolDir.setZero();
        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->velFilter) * qvel_filtered + cfg->velFilter * qvel;
        Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
 
        velocityHorizonList.push_back(currentTwist);
        if (velocityHorizonList.size() > velocityHorizon)
        {
            velocityHorizonList.pop_front();
        }
 
        Eigen::VectorXf targetVel(6);
        Eigen::Vector3f axis;
        Eigen::Vector3f forceInToolFrame;
        Eigen::Vector3f torqueInToolFrame;
        Eigen::Vector6f targetFTInRootFrame;
        Eigen::Vector3f velPInToolFrame;
        targetVel.setZero();
        axis.setZero();
        forceInToolFrame.setZero();
        torqueInToolFrame.setZero();
        targetFTInRootFrame.setZero();
        velPInToolFrame.setZero();
        float angle = 0;
        bool isPhaseStop = false;
 
        if (firstRun || !dmpRunning)
        {
            initHandPose = currentPose;
            lastPosition = currentPose.block<2, 1>(0, 3);
            targetPose = currentPose;
            firstRun = false;
            filteredForce.setZero();
            Eigen::Vector3f currentForce = forceSensor->force - forceOffset;
 
            Eigen::Matrix3f forceFrameOri = rtGetRobot()
                                                ->getRobotNode(cfg->forceFrameName)
                                                ->getPoseInRootFrame()
                                                .block<3, 3>(0, 0);
            Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
            Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
            currentForce = currentForce - handGravity;
 
            currentForceOffset = 0.1 * currentForceOffset + 0.9 * currentForce;
            origHandOri = currentPose.block<3, 3>(0, 0);
            toolTransform = origHandOri.transpose();
            targetVel.setZero();
        }
        else
        {
            rtUpdateControlStruct();
            targetVel = rtGetControlStruct().targetTSVel;
 
 
            Eigen::Matrix3f currentToolOri = currentPose.block<3, 3>(0, 0) * toolTransform;
 
            /* -------------------------- get target vel from dmp thread --------------------------------- */
            targetVel(2) = 0;
            targetVel.head<3>() = currentToolOri * targetVel.head<3>();
            targetVel.tail<3>() = currentToolOri * targetVel.tail<3>();
 
            double canVal = rtGetControlStruct().canVal;
            if (canVal - lastCanVal > 0.9 * cfg->timeDuration)
            {
                wipingCounter++;
                mu = 1.0;
            }
            lastCanVal = canVal;
 
            /* -------------------------- force feedback, filter and transform frame --------------------------------- */
            Eigen::Vector3f currentForce = forceSensor->force - forceOffset - currentForceOffset;
 
            Eigen::Matrix3f forceFrameOri = rtGetRobot()
                                                ->getRobotNode(cfg->forceFrameName)
                                                ->getPoseInRootFrame()
                                                .block<3, 3>(0, 0);
            Eigen::Vector3f gravityInForceFrame = forceFrameOri.transpose() * gravityInRoot;
            Eigen::Vector3f handGravity = handMass * gravityInForceFrame;
 
            currentForce = currentForce - handGravity;
            filteredForce =
                (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * currentForce;
 
            Eigen::Vector3f currentTorque = forceSensor->torque - torqueOffset;
            Eigen::Vector3f handTorque = handCOM.cross(gravityInForceFrame);
            currentTorque = currentTorque - handTorque;
            filteredTorque =
                (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * currentTorque;
 
            Eigen::Vector3f forceInRootForFricEst = forceFrameOri * filteredForce;
            Eigen::Vector3f forceInToolForFricEst =
                currentToolOri.transpose() * forceInRootForFricEst;
 
            for (size_t i = 0; i < 3; ++i)
            {
                if (fabs(filteredForce(i)) > cfg->forceDeadZone)
                {
                    filteredForce(i) -=
                        (filteredForce(i) / fabs(filteredForce(i))) * cfg->forceDeadZone;
                }
                else
                {
                    filteredForce(i) = 0;
                }
            }
 
            filteredForceInRoot = forceFrameOri * filteredForce;
            filteredTorqueInRoot = forceFrameOri * filteredTorque;
            float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
 
            forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
            // TODO this is wrong
            torqueInToolFrame = currentToolOri.transpose() * filteredTorqueInRoot;
            velPInToolFrame = currentToolOri.transpose() * currentTwist.head<3>();
            //            Eigen::Vector3f velRInToolFrame = currentToolOri.transpose() * currentTwist.tail<3>();
 
 
            /* -------------------------- Drag Force Adaptation --------------------------------- */
            //            Eigen::Vector3f dragForce = filteredForceInRoot - cfg->dragForceDeadZone * filteredForceInRoot / filteredForceInRoot.norm();
            if (abnormalFlag == true && filteredForceInRoot.norm() > cfg->dragForceDeadZone)
            {
                //                adaptKpForce = fmax(adaptKpForce - dTf * cfg->decreaseKpForceCoeff, 0);
                //                adaptKpRot = fmax(adaptKpRot - dTf * cfg->decreaseKpRotCoeff, 0);
                adaptKpForce *= cfg->adaptRateDecrease;
                adaptKpRot *= cfg->adaptRateDecreaseRot;
                forcePID->Kp = adaptKpForce;
                torquePID->Kp = adaptKpRot;
                //                adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
                //                adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
                //                adaptK(2) = fmax(adaptK(2) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
                //                adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff, 0);
                //                adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff, 0);
                //                adaptK(2) = fmax(adaptK(2) - dTf * cfg->adaptForceCoeff, 0);
                adaptK *= cfg->adaptRateDecrease;
                adaptD *= cfg->adaptRateDecrease;
                if (cfg->isAdaptOriImpEnabled)
                {
                    adaptKOri *= cfg->adaptRateDecrease;
                    adaptDOri *= cfg->adaptRateDecrease;
                }
                adaptKNull *= cfg->adaptRateDecrease;
                adaptDNull *= cfg->adaptRateDecrease;
 
                positionOffset.setZero();
                forcePID->reset();
 
                //                lastDiff = diff;
            }
            else
            {
                adaptKpForce =
                    fmin(adaptKpForce + dTf * cfg->increaseKpForceCoeff, cfg->pidForce[0]);
                adaptKpRot = fmin(adaptKpRot + dTf * cfg->increaseKpRotCoeff, cfg->pidRot[0]);
                //                adaptKpForce *= cfg->adaptRateIncrease;
                //                adaptKpRot *= cfg->adaptRateIncrease;
                forcePID->Kp = adaptKpForce;
                torquePID->Kp = adaptKpRot;
                for (int i = 0; i < 3; i++)
                {
                    adaptK(i) = fmin(adaptK(i) + fabs(dTf * cfg->adaptCoeff), kpos(i));
                    adaptD(i) = fmin(adaptD(i) + fabs(dTf * cfg->adaptCoeffKdImpIncrease), dpos(i));
                    if (cfg->isAdaptOriImpEnabled)
                    {
                        adaptKOri(i) =
                            fmin(adaptKOri(i) + fabs(dTf * cfg->increaseKpOriCoeff), kori(i));
                        adaptDOri(i) =
                            fmin(adaptDOri(i) + fabs(dTf * cfg->increaseKdOriCoeff), dori(i));
                    }
                }
                for (size_t i = 0; i < targets.size(); i++)
                {
                    adaptKNull(i) =
                        fmin(adaptKNull(i) + fabs(dTf * cfg->increaseKpNullCoeff), knull(i));
                    adaptDNull(i) =
                        fmin(adaptDNull(i) + fabs(dTf * cfg->increaseKdNullCoeff), dnull(i));
                }
 
                //                adaptK *= cfg->adaptRateIncrease;
                //                adaptD *= cfg->adaptRateIncrease;
            }
 
            if (!isContactedOnce && fabs(forceInToolFrame(2)) > targetForce)
            {
                isContactedOnce = true;
            }
            if (cfg->loseContactDetectionEnabled && isContactedOnce)
            {
                if (abnormalFlag && !lastAbnormalFlag)
                {
                    startLoseContactDetection = true;
                    loseContactCounter = 0;
                    forceIntegral = 0;
                }
                if (startLoseContactDetection && loseContactCounter < cfg->loseContactCounterMax)
                {
                    forceIntegral += forceInToolFrame.norm() * deltaT;
                    loseContactCounter++;
                }
                if (loseContactCounter >= cfg->loseContactCounterMax &&
                    forceIntegral < cfg->loseContactForceIntThreshold)
                {
                    isLoseContact = true;
                }
                lastAbnormalFlag = abnormalFlag;
                if (isLoseContact)
                {
                    adaptK *= 0.0;
                    adaptD *= 0.0;
                    adaptKOri *= 0.0;
                    adaptDOri *= 0.0;
                    adaptKNull *= 0.0;
                    adaptDNull *= 0.0;
                }
            }
 
            //            adaptK(2) = kpos(2);
            /* -------------------------- friction estimation --------------------------------- */
 
            Eigen::Vector2f v_xy;
            Eigen::Vector2f f_xy;
            v_xy << velPInToolFrame(0), velPInToolFrame(1);
            f_xy << forceInToolForFricEst(0), forceInToolForFricEst(1);
            f_xy = cfg->fricEstiFilter * f_xy + (1 - cfg->fricEstiFilter) * lastForceInToolXY;
            lastForceInToolXY = f_xy;
 
            if (wipingCounter > 0)
            {
                if (v_xy.norm() > cfg->velNormThreshold &&
                    fabs(forceInToolForFricEst(2) - targetForce) < 0.5 * targetForce)
                {
                    recordFrictionNorm.push_back(f_xy.norm());
                    recordForceNormToSurface.push_back(forceInToolForFricEst(2));
                }
                if (recordFrictionNorm.size() > frictionHorizon)
                {
                    recordFrictionNorm.pop_front();
                    recordForceNormToSurface.pop_front();
                    float dotProduct = 0.0;
                    float normSquare = 0.0;
                    for (size_t i = 0; i < recordFrictionNorm.size(); i++)
                    {
                        dotProduct += (recordFrictionNorm[i] * recordForceNormToSurface[i]);
                        normSquare += (recordForceNormToSurface[i] * recordForceNormToSurface[i]);
                    }
                    if (normSquare > 0)
                    {
                        float mu_tmp = dotProduct / normSquare;
                        if (mu_tmp > 0)
                        {
                            mu = fmax(fmin(mu, mu_tmp), safeFrictionConeLowerLimit);
                        }
                    }
                    if (v_xy.norm() > cfg->velNormThreshold)
                    {
                        estimatedFriction = -v_xy * mu * forceInToolForFricEst(2) / v_xy.norm();
                    }
                }
            }
 
            /* -------------------------- Force Regulation and Torque PID Controller --------------------------------- */
 
            if (isForceCtrlInForceDir)
            {
                forcePID->update(deltaT, forceInToolFrame.norm(), targetForce);
            }
            else
            {
                forcePID->update(deltaT, forceInToolFrame(2), targetForce);
            }
            torquePID->update(deltaT, torqueInToolFrame(1), 0.0);
 
            /* -------------------------- Rotation PID Controller --------------------------------- */
 
            currentToolDir = currentToolOri * oriToolDir;
            for (int i = 3; i < 6; ++i)
            {
                targetVel(i) = 0;
            }
            float frictionCone;
            if (cfg->frictionCone < 0.0)
            {
                frictionCone = atan(mu);
            }
            else
            {
                frictionCone = cfg->frictionCone;
            }
            float adaptedAngularKp = 0.0;
            Eigen::Vector3f angularDiff;
            angularDiff.setZero();
            // rotation changes
            if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
            {
                Eigen::Vector3f toolYDir;
                toolYDir << 0, 1.0, 0;
                Eigen::Vector3f toolYDirInRoot = currentToolOri * toolYDir;
                Eigen::Vector3f projectedFilteredForceInRoot =
                    filteredForceInRoot - filteredForceInRoot.dot(toolYDirInRoot) * toolYDirInRoot;
                Eigen::Vector3f desiredToolDir =
                    projectedFilteredForceInRoot
                        .normalized(); // / projectedFilteredForceInRoot.norm();
                currentToolDir.normalize();
 
                axis = currentToolDir.cross(desiredToolDir);
                axis = axis.normalized();
                angle = acosf(currentToolDir.dot(desiredToolDir));
 
                int sign = 1;
                if (axis(1) < 0)
                {
                    sign = -1;
                }
 
                if (fabs(angle) < M_PI / 2 && fabs(angle) > frictionCone)
                {
                    // sigmoid function
                    adaptedAngularKp =
                        cfg->pidRot[0] / (1 + exp(10 * (angle - cfg->rotAngleSigmoid)));
                    adaptedAngularKp = fmin(adaptedAngularKp, cfg->pidRot[0]);
                    rotPID->Kp = adaptedAngularKp;
                    angle -= frictionCone;
                    angle *= sign;
                }
                else
                {
                    angle = 0.0;
                    rotPID->Kp = cfg->pidRot[0];
                }
            }
            rotPID->update(deltaT, angle, 0.0);
 
            /* -------------------------- Lose Contact Recover PID Controller --------------------------------- */
 
            //            if (forceInToolFrame(2) > loseContactRatio * targetForce)
            //            {
            //                makingContactCounter++;
            //                if (makingContactCounter > 100)
            //                {
            //                    isMakingContact = true;
            //                    isLCREnabled = false;
            //                }
            //                else
            //                {
            //                    isMakingContact = false;
            //                }
            //            }
            //            if (!isContactedOnce && isMakingContact)
            //            {
            //                isContactedOnce = true;
            //            }
            //            Eigen::Vector3f compensationAxis;
            //            compensationAxis.setZero();
            //            if (isContactedOnce && fabs(velPInToolFrame(2)) > 10 && frictionCone < 1.0)
            //            {
            //                makingContactCounter = 0;
            //                Eigen::Vector3f v;
            //                v << velPInToolFrame(0), 0.0, 0.0;
            //                Eigen::Vector3f f;
            //                f << 0.0, 0.0, targetForce;
            //                compensationAxis = f.cross(v);
            //                if (compensationAxis.norm() > 0)
            //                {
            //                    compensationAxis.normalized();
            //                }
            //                else
            //                {
            //                    compensationAxis.setZero();
            //                }
            //                forceControlGate *= 0.5;
            //                isLCREnabled = true;
            //                lcrCounter -= 1;
            //            }
            //            else
            //            {
            //                forceControlGate = 1.0;
            //                // TODO: restart vmp controller
            //            }
            //            float velInForceDir = 0.0;
            //            if (lcrCounter < 500)
            //            {
            //                velInForceDir = fabs(velPInToolFrame(2));
            //                lcrCounter -= 1;
            //                if (lcrCounter == 0)
            //                {
            //                    lcrCounter = 500;
            //                }
            //            }
            //            lcrPID->update(deltaT, velInForceDir, 0.0);
 
            /* -------------------------- VMP Phase Stop --------------------------------- */
 
            float diff = (targetPose.block<2, 1>(0, 3) - currentPose.block<2, 1>(0, 3)).norm();
            if (diff > cfg->phaseDist0)
            {
                isPhaseStop = true;
            }
 
 
            /* -------------------------- get PID control commands --------------------------------- */
            //            if (isLCRControlEnabled)
            //            {
            //                //                targetFTInToolFrame.tail<3>() += (float)lcrPID->getControlValue() * compensationAxis;
            ////                targetVel.tail<3>() += (float)lcrPID->getControlValue() * compensationAxis;
            //            }
 
            if (isForceControlEnabled)
            {
                if (isForceCtrlInForceDir)
                {
                    float forcePIDVel = -(float)forcePID->getControlValue();
                    Eigen::Vector3f targetVelInTool;
                    if (forceInToolFrame.norm() < 1e-4)
                    {
                        targetVelInTool << 0, 0, forcePIDVel;
                    }
                    else
                    {
                        targetVelInTool = forceInToolFrame / forceInToolFrame.norm() * forcePIDVel;
                    }
                    targetVel.head<3>() += currentToolOri * targetVelInTool;
                }
                else
                {
                    //                targetFTInToolFrame(2) -= (float)forcePID->getControlValue() * forceControlGate;
                    Eigen::Vector3f localVel;
                    localVel << 0, 0, -(float)forcePID->getControlValue();
                    positionOffset += currentToolOri * localVel * deltaT;
 
                    targetVel(2) -= (float)forcePID->getControlValue();
                    targetVel.head<3>() = currentToolOri * targetVel.head<3>();
                }
            }
            else
            {
                positionOffset.setZero();
            }
 
            if (isRotControlEnabled)
            {
                //                targetFTInToolFrame(4) -= (float)rotPID->getControlValue();
                //                targetVel.tail<3>() = adaptedAngularKp * angularDiff;
                targetVel(4) -= (float)rotPID->getControlValue();
            }
            if (isTorqueControlEnabled)
            {
                //                targetFTInToolFrame(4) -= (float)torquePID->getControlValue();
                targetVel(4) += (float)torquePID->getControlValue();
            }
 
            //            targetFTInRootFrame.head<3>() = currentToolOri * targetFTInToolFrame.head<3>();
            //            targetFTInRootFrame.tail<3>() = currentToolOri * targetFTInToolFrame.tail<3>();
        }
 
 
        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
        rt2UserData.getWriteBuffer().tcpTranslVel = currentTwist.head<3>();
        rt2UserData.getWriteBuffer().forceOutput = filteredForceInRoot;
        rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
        rt2UserData.commitWrite();
 
        /* -------------------------- Integrate Target Velocity 2 Target Pose --------------------------------- */
 
        if (useDMPInGlobalFrame)
        {
            targetPose = rtGetControlStruct().targetTSPose;
            targetPose.block<3, 1>(0, 3) += positionOffset;
        }
        else
        {
            targetPose.block<3, 1>(0, 3) =
                targetPose.block<3, 1>(0, 3) + dTf * targetVel.block<3, 1>(0, 0);
            Eigen::Matrix3f rotMat = VirtualRobot::MathTools::rpy2eigen3f(
                dTf * targetVel(3), dTf * targetVel(4), dTf * targetVel(5));
            targetPose.block<3, 3>(0, 0) = rotMat * targetPose.block<3, 3>(0, 0);
 
            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) = targetPose(0, 3) > cfg->ws_x[0] ? targetPose(0, 3) : cfg->ws_x[0];
        targetPose(0, 3) = targetPose(0, 3) < cfg->ws_x[1] ? targetPose(0, 3) : cfg->ws_x[1];
 
        targetPose(1, 3) = targetPose(1, 3) > cfg->ws_y[0] ? targetPose(1, 3) : cfg->ws_y[0];
        targetPose(1, 3) = targetPose(1, 3) < cfg->ws_y[1] ? targetPose(1, 3) : cfg->ws_y[1];
 
        targetPose(2, 3) = targetPose(2, 3) > cfg->ws_z[0] ? targetPose(2, 3) : cfg->ws_z[0];
        targetPose(2, 3) = targetPose(2, 3) < cfg->ws_z[1] ? targetPose(2, 3) : cfg->ws_z[1];
 
        /* -------------------------- Force/Torque Impedance Controller --------------------------------- */
 
        // inverse dynamic controller
        jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
 
        Eigen::Matrix3f diffMat =
            targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).transpose();
        Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
        Eigen::Vector6f taskFTControlCommand;
 
        //        for (int i = 0; i < 3; i++)
        //        {
        //            adaptD[i] = (float)2 * sqrt(adaptK[i]);
        //        }
        taskFTControlCommand.head<3>() =
            adaptK.cwiseProduct(targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) *
                0.001 +
            adaptD.cwiseProduct(-currentTwist.head<3>()) * 0.001 + targetFTInRootFrame.head<3>();
        taskFTControlCommand.tail<3>() = kori.cwiseProduct(rpy) +
                                       dori.cwiseProduct(-currentTwist.tail<3>()) +
                                       targetFTInRootFrame.tail<3>();
 
        filteredFTCommand = cfg->ftCommandFilter * taskFTControlCommand +
                            (1 - cfg->ftCommandFilter) * filteredFTCommand;
 
        /* -------------------------- NullSpace and Joint Torque Controller --------------------------------- */
 
        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
        Eigen::VectorXf nullspaceTorque =
            knull.cwiseProduct(nullSpaceJointsVec - qpos) - dnull.cwiseProduct(qvel);
        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
        Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * filteredFTCommand +
                                              (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
        //        if (forceInToolFrame.norm() > cfg->maxInteractionForce)
        //        {
        //            jointDesiredTorques.setZero();
        //        }
 
        for (size_t i = 0; i < targets.size(); ++i)
        {
            targets.at(i)->torque = jointDesiredTorques(i);
 
            if (!targets.at(i)->isValid())
            {
                targets.at(i)->torque = 0.0f;
            }
            else
            {
                if (fabs(targets.at(i)->torque) > fabs(cfg->maxJointTorque))
                {
                    targets.at(i)->torque = fabs(cfg->maxJointTorque) *
                                            (targets.at(i)->torque / fabs(targets.at(i)->torque));
                }
            }
        }
 
        /* -------------------------- Communication --------------------------------- */
 
        debugRT.getWriteBuffer().currentToolDir =
            tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(currentToolDir);
        debugRT.getWriteBuffer().targetPose = targetPose;
        debugRT.getWriteBuffer().globalPose =
            tcp->getRobot()->getRootNode()->toGlobalCoordinateSystem(targetPose);
        debugRT.getWriteBuffer().currentPose = currentPose;
        debugRT.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
        debugRT.getWriteBuffer().rotationAxis = axis;
        debugRT.getWriteBuffer().filteredForce = forceInToolFrame;
        debugRT.getWriteBuffer().globalFilteredForce =
            tcp->getRobot()->getRootNode()->toGlobalCoordinateSystemVec(filteredForceInRoot);
        debugRT.getWriteBuffer().targetVel = targetVel;
        debugRT.getWriteBuffer().adaptK = adaptK;
        debugRT.getWriteBuffer().adaptD = adaptD;
        debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
        debugRT.getWriteBuffer().rotAngle = angle;
        debugRT.getWriteBuffer().currentTwist = currentTwist;
        debugRT.getWriteBuffer().filteredTorque = torqueInToolFrame;
        debugRT.getWriteBuffer().wipingCounter = wipingCounter;
        debugRT.getWriteBuffer().mu = mu;
        debugRT.getWriteBuffer().estimatedFriction = estimatedFriction;
        debugRT.getWriteBuffer().frictionInToolXY = lastForceInToolXY;
        debugRT.getWriteBuffer().velPInTool = velPInToolFrame;
        debugRT.getWriteBuffer().kpForcePID = adaptKpForce;
        debugRT.getWriteBuffer().kpRotPID = adaptKpRot;
        debugRT.getWriteBuffer().loseContactForceIntegral = forceIntegral;
 
 
        debugRT.commitWrite();
 
        rt2CtrlData.getWriteBuffer().currentPose = currentPose;
        rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
        rt2CtrlData.getWriteBuffer().deltaT = deltaT;
        rt2CtrlData.getWriteBuffer().currentTime += deltaT;
        rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
        rt2CtrlData.commitWrite();
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::learnDMPFromFiles(
        const Ice::StringSeq& fileNames,
        const Ice::Current&)
    {
        ARMARX_INFO << "Learning DMP ... ";
 
        LockGuardType guard{controllerMutex};
        dmpCtrl->learnDMPFromFiles(fileNames);
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::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{controllerMutex};
        dmpCtrl->learnDMPFromTrajectory(dmpTraj);
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::setSpeed(Ice::Double times, const Ice::Current&)
    {
        LockGuardType guard{controllerMutex};
        dmpCtrl->setSpeed(times);
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::setGoals(const Ice::DoubleSeq& goals,
                                                             const Ice::Current& ice)
    {
        LockGuardType guard{controllerMutex};
        dmpCtrl->setGoalPoseVec(goals);
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::setAmplitude(Ice::Double amp,
                                                                 const Ice::Current&)
    {
        LockGuardType guard{controllerMutex};
        dmpCtrl->setAmplitude(amp);
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::setTargetForceInRootFrame(float targetForce,
                                                                              const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        user2rtData.getWriteBuffer().targetForce = targetForce;
        user2rtData.commitWrite();
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::runDMP(const Ice::DoubleSeq& goals,
                                                           Ice::Double tau,
                                                           const Ice::Current&)
    {
        firstRun = true;
        while (firstRun || rt2UserData.getUpToDateReadBuffer().waitTimeForCalibration <
                               cfg->waitTimeForCalibration)
        {
            usleep(100);
        }
 
 
        Eigen::Matrix4f pose = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
 
        LockGuardType guard{controllerMutex};
        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
        dmpCtrl->setSpeed(tau);
 
        ARMARX_IMPORTANT << "run DMP";
        started = true;
        dmpRunning = false;
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::setTrigerAbnormalEvent(bool abnormal,
                                                                           const Ice::Current&)
    {
        abnormalFlag = abnormal;
    }
 
    std::vector<float>
    NJointAnomalyDetectionAdaptiveWipingController::getAnomalyInput(const Ice::Current&)
    {
        Eigen::Matrix4f tpos = rt2UserData.getUpToDateReadBuffer().currentTcpPose;
        Eigen::Vector3f tvel = rt2UserData.getUpToDateReadBuffer().tcpTranslVel;
        std::vector<float> tvelvec = {
            tvel(0), tvel(1), tvel(2), tpos(0, 3) / 1000, tpos(1, 3) / 1000, tpos(2, 3) / 1000};
        return tvelvec;
    }
 
    std::vector<float>
    NJointAnomalyDetectionAdaptiveWipingController::getAnomalyOutput(const Ice::Current&)
    {
        Eigen::Vector3f force = rt2UserData.getUpToDateReadBuffer().forceOutput;
        std::vector<float> forceVec = {force(0), force(1), force(2)};
        return forceVec;
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::onPublish(
        const SensorAndControl&,
        const DebugDrawerInterfacePrx& debugDrawer,
        const DebugObserverInterfacePrx& debugObs)
    {
        std::string datafieldName;
        std::string debugName = "Periodic";
        StringVariantBaseMap datafields;
 
        Eigen::Matrix4f targetPoseDebug = debugRT.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));
 
        VirtualRobot::MathTools::Quaternion qtarget =
            VirtualRobot::MathTools::eigen4f2quat(targetPoseDebug);
        datafields["target_qw"] = new Variant(qtarget.w);
        datafields["target_qx"] = new Variant(qtarget.x);
        datafields["target_qy"] = new Variant(qtarget.y);
        datafields["target_qz"] = new Variant(qtarget.z);
 
 
        Eigen::Matrix4f currentPoseDebug = debugRT.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));
 
 
        VirtualRobot::MathTools::Quaternion qcurrent =
            VirtualRobot::MathTools::eigen4f2quat(currentPoseDebug);
        datafields["current_qw"] = new Variant(qcurrent.w);
        datafields["current_qx"] = new Variant(qcurrent.x);
        datafields["current_qy"] = new Variant(qcurrent.y);
        datafields["current_qz"] = new Variant(qcurrent.z);
 
 
        Eigen::Vector3f filteredForce = debugRT.getUpToDateReadBuffer().filteredForce;
        datafields["filteredforceInTool_x"] = new Variant(filteredForce(0));
        datafields["filteredforceInTool_y"] = new Variant(filteredForce(1));
        datafields["filteredforceInTool_z"] = new Variant(filteredForce(2));
 
        Eigen::Vector3f filteredTorque = debugRT.getUpToDateReadBuffer().filteredTorque;
        datafields["filteredtorqueInTool_x"] = new Variant(filteredTorque(0));
        datafields["filteredtorqueInTool_y"] = new Variant(filteredTorque(1));
        datafields["filteredtorqueInTool_z"] = new Variant(filteredTorque(2));
 
 
        Eigen::Vector3f filteredForceInRoot = debugRT.getUpToDateReadBuffer().filteredForceInRoot;
        datafields["filteredForceInRoot_x"] = new Variant(filteredForceInRoot(0));
        datafields["filteredForceInRoot_y"] = new Variant(filteredForceInRoot(1));
        datafields["filteredForceInRoot_z"] = new Variant(filteredForceInRoot(2));
 
        Eigen::Vector3f rotationAxis = debugRT.getUpToDateReadBuffer().rotationAxis;
        datafields["rotationAxis_x"] = new Variant(rotationAxis(0));
        datafields["rotationAxis_y"] = new Variant(rotationAxis(1));
        datafields["rotationAxis_z"] = new Variant(rotationAxis(2));
 
        Eigen::Vector3f reactForce = debugRT.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 = debugRT.getUpToDateReadBuffer().targetVel;
        datafields["targetVel_x"] = new Variant(targetVel(0));
        datafields["targetVel_y"] = new Variant(targetVel(1));
        datafields["targetVel_z"] = new Variant(targetVel(2));
        datafields["targetVel_ro"] = new Variant(targetVel(3));
        datafields["targetVel_pi"] = new Variant(targetVel(4));
        datafields["targetVel_ya"] = new Variant(targetVel(5));
 
        Eigen::VectorXf currentTwist = debugRT.getUpToDateReadBuffer().currentTwist;
        datafields["currentTwist_x"] = new Variant(currentTwist(0));
        datafields["currentTwist_y"] = new Variant(currentTwist(1));
        datafields["currentTwist_z"] = new Variant(currentTwist(2));
        datafields["currentTwist_ro"] = new Variant(currentTwist(3));
        datafields["currentTwist_pi"] = new Variant(currentTwist(4));
        datafields["currentTwist_ya"] = new Variant(currentTwist(5));
 
 
        Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
        datafields["adaptK_x"] = new Variant(adaptK(0));
        datafields["adaptK_y"] = new Variant(adaptK(1));
        datafields["adaptK_z"] = new Variant(adaptK(2));
 
        Eigen::Vector3f adaptD = debugRT.getUpToDateReadBuffer().adaptD;
        datafields["adaptD_x"] = new Variant(adaptD(0));
        datafields["adaptD_y"] = new Variant(adaptD(1));
        datafields["adaptD_z"] = new Variant(adaptD(2));
 
        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
        datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
 
        datafields["PhaseStop"] = new Variant(debugRT.getUpToDateReadBuffer().isPhaseStop);
        datafields["rotAngle"] = new Variant(debugRT.getUpToDateReadBuffer().rotAngle);
        datafields["wipingCounter"] = new Variant(debugRT.getUpToDateReadBuffer().wipingCounter);
        datafields["mu"] = new Variant(debugRT.getUpToDateReadBuffer().mu);
        datafields["loseContactForceIntegral"] =
            new Variant(debugRT.getUpToDateReadBuffer().loseContactForceIntegral);
 
        Eigen::VectorXf estFri = debugRT.getUpToDateReadBuffer().estimatedFriction;
        datafields["estimatedFriction_x"] = new Variant(estFri(0));
        datafields["estimatedFriction_y"] = new Variant(estFri(1));
 
        Eigen::VectorXf frictionInToolXY = debugRT.getUpToDateReadBuffer().frictionInToolXY;
        datafields["frictionInToolXY_x"] = new Variant(frictionInToolXY(0));
        datafields["frictionInToolXY_y"] = new Variant(frictionInToolXY(1));
 
        Eigen::VectorXf velPInTool = debugRT.getUpToDateReadBuffer().velPInTool;
        datafields["velPInTool_x"] = new Variant(velPInTool(0));
        datafields["velPInTool_y"] = new Variant(velPInTool(1));
        datafields["velPInTool_z"] = new Variant(velPInTool(2));
 
        datafields["kp_force_pid"] = new Variant(debugRT.getUpToDateReadBuffer().kpForcePID);
        datafields["kp_rot_pid"] = new Variant(debugRT.getUpToDateReadBuffer().kpRotPID);
 
        //        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);
 
 
        // draw force;
        Eigen::Matrix4f globalPose = debugRT.getUpToDateReadBuffer().globalPose;
        Eigen::Vector3f handPosition = globalPose.block<3, 1>(0, 3);
        Eigen::Vector3f forceDir = debugRT.getUpToDateReadBuffer().globalFilteredForce;
 
        debugDrawer->setArrowVisu("Force",
                                  "currentForce",
                                  new Vector3(handPosition),
                                  new Vector3(forceDir),
                                  DrawColor{0, 0, 1, 1},
                                  10 * forceDir.norm(),
                                  3);
 
        // draw direction of the tool
        Eigen::Vector3f currentToolDir = debugRT.getUpToDateReadBuffer().currentToolDir;
        debugDrawer->setArrowVisu("Tool",
                                  "Tool",
                                  new Vector3(handPosition),
                                  new Vector3(currentToolDir),
                                  DrawColor{1, 0, 0, 1},
                                  100,
                                  3);
        debugDrawer->setPoseVisu("target", "targetPose", new Pose(globalPose));
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::pauseDMP(const Ice::Current&)
    {
        dmpCtrl->pauseController();
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::resumeDMP(const Ice::Current&)
    {
        dmpCtrl->resumeController();
    }
 
    void
    NJointAnomalyDetectionAdaptiveWipingController::onDisconnectNJointController()
    {
        ARMARX_INFO << "stopped ...";
    }
 
 
} // namespace armarx::control::deprecated_njoint_mp_controller::adaptive