NJointAdaptiveWipingController.cpp

Go to the documentation of this file.

#include "NJointAdaptiveWipingController.h"
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
namespace armarx::control::deprecated_njoint_mp_controller::adaptive
{
    NJointControllerRegistration<NJointAdaptiveWipingController> registrationControllerNJointAdaptiveWipingController("NJointAdaptiveWipingController");
 
    NJointAdaptiveWipingController::NJointAdaptiveWipingController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
    {
        useSynchronizedRtRobot();
        cfg =  NJointAdaptiveWipingControllerConfigPtr::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);
        };
 
        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;
 
 
 
        dmpCtrl.reset(new tsvmp::TaskSpaceDMPController("periodicDMP", taskSpaceDMPConfig, false));
 
        NJointAdaptiveWipingControllerControlData initData;
        initData.targetTSVel.resize(6);
        for (size_t i = 0; i < 6; ++i)
        {
            initData.targetTSVel(i) = 0;
        }
        reinitTripleBuffer(initData);
 
        firstRun = true;
        dmpRunning = false;
 
 
        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];
 
        kpf = cfg->Kpf;
        //        forcePID.reset(new PIDController(cfg->kpf, ));
        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();
 
        filteredForceInRoot.setZero();
        filteredTorqueInRoot.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;
        lastDiff = 0;
        changeTimer = 0;
    }
 
    void NJointAdaptiveWipingController::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("NJointAdaptiveWipingController", [&]
        {
            CycleUtil c(1);
            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
            while (getState() == eManagedIceObjectStarted)
            {
                if (isControllerActive())
                {
                    controllerRun();
                }
                c.waitForCycleDuration();
            }
        });
 
    }
 
    std::string NJointAdaptiveWipingController::getClassName(const Ice::Current&) const
    {
        return "NJointAdaptiveWipingController";
    }
 
    void NJointAdaptiveWipingController::controllerRun()
    {
        if (!started)
        {
            return;
        }
 
        if (!dmpCtrl)
        {
            return;
        }
 
        Eigen::VectorXf targetVels(6);
        bool isPhaseStop = rt2CtrlData.getUpToDateReadBuffer().isPhaseStop;
        if (isPhaseStop)
        {
            targetVels.setZero();
        }
        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();
 
            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().targetTSVel = targetVels;
        writeControlStruct();
 
        dmpRunning = true;
    }
 
 
    void NJointAdaptiveWipingController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
    {
        double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
        rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
        rt2UserData.commitWrite();
 
        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;
 
        Eigen::VectorXf targetVel(6);
        Eigen::Vector3f axis;
        axis.setZero();
        targetVel.setZero();
        Eigen::Vector3f forceInToolFrame;
        forceInToolFrame << 0, 0, 0;
 
        Eigen::Vector3f torqueInToolFrame;
        torqueInToolFrame << 0, 0, 0;
 
        float angle = 0;
        if (firstRun || !dmpRunning)
        {
            lastPosition = currentPose.block<2, 1>(0, 3);
            targetPose = currentPose;
            firstRun = false;
            filteredForce.setZero();
            Eigen::Vector3f currentForce = forceSensor->force - forceOffset;
 
            Eigen::Matrix3f forceFrameOri =  rtGetRobot()->getSensor(cfg->forceSensorName)->getRobotNode()->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
        {
            // communicate with DMP controller
            rtUpdateControlStruct();
            targetVel = rtGetControlStruct().targetTSVel;
            targetVel(2) = 0;
 
            // calculate force
            Eigen::Vector3f currentForce = forceSensor->force - forceOffset - currentForceOffset;
 
            Eigen::Matrix3f forceFrameOri =  rtGetRobot()->getSensor(cfg->forceSensorName)->getRobotNode()->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;
 
            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;
 
            Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
            Eigen::Matrix3f currentToolOri = currentHandOri * toolTransform;
 
            forceInToolFrame = currentToolOri.transpose() * filteredForceInRoot;
            torqueInToolFrame = currentToolOri.transpose() * filteredTorqueInRoot;
 
            float desiredZVel = kpf * (targetForce - forceInToolFrame(2));
            targetVel(2) -= desiredZVel;
            targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
 
            currentToolDir = currentToolOri * oriToolDir;
 
            for (int i = 3; i < 6; ++i)
            {
                targetVel(i) = 0;
            }
 
            // 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));
 
 
                if (fabs(angle) < M_PI / 2 && fabs(angle) > cfg->frictionCone)
                {
                    // sigmoid function
                    float adaptedAngularKp = cfg->angularKp / (1 +  exp(10 * (angle - M_PI / 4)));
                    float angularKp = fmin(adaptedAngularKp, cfg->angularKp);
 
                    // test axis
                    Eigen::Vector3f fixedAxis;
                    if (axis(1) > 0)
                    {
                        fixedAxis << 0.0, 1.0, 0.0;
                    }
                    else
                    {
                        fixedAxis << 0.0, -1.0, 0.0;
                    }
                    Eigen::AngleAxisf desiredToolRot(angle - cfg->frictionCone, fixedAxis);
                    Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
 
                    Eigen::Vector3f angularDiff = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
 
                    targetVel.tail(3) = angularKp * angularDiff;
 
                    //Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
                    Eigen::Vector3f checkedToolDir =  desiredRotMat * currentToolDir;
                    checkedToolDir.normalize();
                }
            }
            // integrate for 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;
        }
 
        float dTf = (float)deltaT;
 
 
        if (filteredForceInRoot.block<2, 1>(0, 0).norm() > cfg->dragForceDeadZone)
        {
            Eigen::Vector2f dragForce = filteredForceInRoot.block<2, 1>(0, 0) - cfg->dragForceDeadZone * filteredForceInRoot.block<2, 1>(0, 0) / filteredForceInRoot.block<2, 1>(0, 0).norm();
            adaptK(0) = fmax(adaptK(0) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
            adaptK(1) = fmax(adaptK(1) - dTf * cfg->adaptForceCoeff * dragForce.norm(), 0);
            lastDiff = diff;
        }
        else
        {
            adaptK(0) = fmin(adaptK(0) + fabs(dTf * cfg->adaptCoeff), kpos(0));
            adaptK(1) = fmin(adaptK(1) + fabs(dTf * cfg->adaptCoeff), kpos(1));
        }
        adaptK(2) = kpos(2);
 
        // adaptive control with distance
 
 
 
 
        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];
 
 
 
        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().isPhaseStop = isPhaseStop;
        debugRT.getWriteBuffer().rotAngle = angle;
        debugRT.getWriteBuffer().currentTwist = currentTwist;
        debugRT.getWriteBuffer().filteredTorque = torqueInToolFrame;
 
 
        debugRT.commitWrite();
 
        rt2CtrlData.getWriteBuffer().currentPose = currentPose;
        rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
        rt2CtrlData.getWriteBuffer().deltaT = deltaT;
        rt2CtrlData.getWriteBuffer().currentTime += deltaT;
        rt2CtrlData.getWriteBuffer().isPhaseStop = isPhaseStop;
        rt2CtrlData.commitWrite();
 
        //            Eigen::Matrix3f rotVel = VirtualRobot::MathTools::rpy2eigen3f(targetVel(3) * dTf, targetVel(4) * dTf, targetVel(5) * dTf);
        //            targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
 
        // inverse dynamic controller
        jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
 
 
 
 
        Eigen::Vector6f jointControlWrench;
        {
            Eigen::Vector3f targetTCPLinearVelocity;
            targetTCPLinearVelocity << 0.001 * targetVel(0), 0.001 * targetVel(1), 0.001 * targetVel(2);
            Eigen::Vector3f currentTCPLinearVelocity;
            currentTCPLinearVelocity <<  0.001 * currentTwist(0),  0.001 * currentTwist(1),   0.001 * currentTwist(2);
            Eigen::Vector3f currentTCPPosition = currentPose.block<3, 1>(0, 3);
            Eigen::Vector3f desiredPosition = targetPose.block<3, 1>(0, 3);
 
            Eigen::Vector3f linearVel = adaptK.cwiseProduct(desiredPosition - currentTCPPosition);
 
            //            if (isPhaseStop)
            //            {
            //                linearVel = ((float)cfg->phaseKpPos) * (desiredPosition - currentTCPPosition);
            //                for (size_t i = 0; i < 3; ++i)
            //                {
            //                    linearVel(i) = fmin(cfg->maxLinearVel, linearVel(i));
            //                }
            //            }
            //            else
            //            {
            //                linearVel = kpos.cwiseProduct(desiredPosition - currentTCPPosition);
            //            }
            Eigen::Vector3f tcpDesiredForce = 0.001 * linearVel + dpos.cwiseProduct(- currentTCPLinearVelocity);
 
            Eigen::Vector3f currentTCPAngularVelocity;
            currentTCPAngularVelocity << currentTwist(3),   currentTwist(4),  currentTwist(5);
            Eigen::Matrix3f currentRotMat = currentPose.block<3, 3>(0, 0);
            Eigen::Matrix3f diffMat = targetPose.block<3, 3>(0, 0) * currentRotMat.inverse();
            Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
            Eigen::Vector3f tcpDesiredTorque = kori.cwiseProduct(rpy) + dori.cwiseProduct(- currentTCPAngularVelocity);
            jointControlWrench <<  tcpDesiredForce, tcpDesiredTorque;
        }
 
 
 
        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() * jointControlWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
 
        // torque filter (maybe)
        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));
                }
            }
        }
 
 
    }
 
 
    void NJointAdaptiveWipingController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
    {
        ARMARX_INFO << "Learning DMP ... ";
 
        LockGuardType guard {controllerMutex};
        dmpCtrl->learnDMPFromFiles(fileNames);
 
    }
 
    void NJointAdaptiveWipingController::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 NJointAdaptiveWipingController::setSpeed(Ice::Double times, const Ice::Current&)
    {
        LockGuardType guard {controllerMutex};
        dmpCtrl->setSpeed(times);
    }
 
 
    void NJointAdaptiveWipingController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
    {
        LockGuardType guard {controllerMutex};
        dmpCtrl->setGoalPoseVec(goals);
    }
 
    void NJointAdaptiveWipingController::setAmplitude(Ice::Double amp, const Ice::Current&)
    {
        LockGuardType guard {controllerMutex};
        dmpCtrl->setAmplitude(amp);
    }
 
    void NJointAdaptiveWipingController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
    {
        LockGuardType guard {controlDataMutex};
        user2rtData.getWriteBuffer().targetForce = targetForce;
        user2rtData.commitWrite();
    }
 
    void NJointAdaptiveWipingController::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 NJointAdaptiveWipingController::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));
 
        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));
 
        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["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["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 NJointAdaptiveWipingController::onDisconnectNJointController()
    {
        ARMARX_INFO << "stopped ...";
    }
 
 
 
}