NJointPeriodicTSDMPCompliantController.cpp

Go to the documentation of this file.

#include "NJointPeriodicTSDMPCompliantController.h"
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
    NJointControllerRegistration<NJointPeriodicTSDMPCompliantController> registrationControllerNJointPeriodicTSDMPCompliantController("NJointPeriodicTSDMPCompliantController");
 
    NJointPeriodicTSDMPCompliantController::NJointPeriodicTSDMPCompliantController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
    {
        useSynchronizedRtRobot();
        cfg =  NJointPeriodicTSDMPCompliantControllerConfigPtr::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));
 
        NJointPeriodicTSDMPCompliantControllerControlData 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;
        knull = cfg->Knull;
        dnull = cfg->Dnull;
 
 
 
        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>();
 
        forceOffset.setZero();
        filteredForce.setZero();
        filteredForceInRoot.setZero();
 
 
        UserToRTData initUserData;
        initUserData.targetForce = 0;
        user2rtData.reinitAllBuffers(initUserData);
 
        oriToolDir << 0, 0, 1;
 
        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)
        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]);
        }
 
        adaptK = kpos;
        lastDiff = 0;
        changeTimer = 0;
 
 
        isManipulability = cfg->isManipulability;
 
 
        ARMARX_CHECK_EQUAL(cfg->maniWeight.size(), rns->getNodeNames().size());
        Eigen::VectorXd maniWeightVec;
        maniWeightVec.setOnes(rns->getNodeNames().size());
        for (size_t i = 0; i < cfg->maniWeight.size(); ++i)
        {
            maniWeightVec(i) = cfg->maniWeight[i];
        }
 
        Eigen::MatrixXd maniWeightMat = maniWeightVec.asDiagonal();
        VirtualRobot::SingleRobotNodeSetManipulabilityPtr manipulability(
            new VirtualRobot::SingleRobotNodeSetManipulability(rns, rns->getTCP(),
                    VirtualRobot::AbstractManipulability::Mode::Position,
                    VirtualRobot::AbstractManipulability::Type::Velocity,
                    rns->getRobot()->getRootNode(), maniWeightMat));
        manipulabilityTracker.reset(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
 
        ARMARX_CHECK_EQUAL(cfg->positionManipulability.size(), 9);
        targetManipulability.setZero(3, 3);
        targetManipulability << cfg->positionManipulability[0],  cfg->positionManipulability[1], cfg->positionManipulability[2],
                             cfg->positionManipulability[3],  cfg->positionManipulability[4], cfg->positionManipulability[5],
                             cfg->positionManipulability[6],  cfg->positionManipulability[7], cfg->positionManipulability[8];
 
 
        Eigen::VectorXd kmaniVec;
        kmaniVec.setZero(cfg->kmani.size());
        for (size_t i = 0; i < cfg->kmani.size(); ++i)
        {
            kmaniVec[i] = cfg->kmani[i];
        }
 
        kmani = kmaniVec.asDiagonal();
 
    }
 
    void NJointPeriodicTSDMPCompliantController::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);
 
 
        ARMARX_IMPORTANT << "read force sensor ...";
 
        forceOffset = forceSensor->force;
 
        ARMARX_IMPORTANT << "force offset: " << forceOffset;
 
        started = false;
 
        runTask("NJointPeriodicTSDMPCompliantController", [&]
        {
            CycleUtil c(1);
            getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
            while (getState() == eManagedIceObjectStarted)
            {
                if (isControllerActive())
                {
                    controllerRun();
                }
                c.waitForCycleDuration();
            }
        });
 
        ARMARX_IMPORTANT << "started controller ";
 
    }
 
    std::string NJointPeriodicTSDMPCompliantController::getClassName(const Ice::Current&) const
    {
        return "NJointPeriodicTSDMPCompliantController";
    }
 
    void NJointPeriodicTSDMPCompliantController::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 NJointPeriodicTSDMPCompliantController::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::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);
        targetVel.setZero();
        if (firstRun || !dmpRunning)
        {
            lastPosition = currentPose.block<2, 1>(0, 3);
            targetPose = currentPose;
            firstRun = false;
            filteredForce.setZero();
 
            origHandOri = currentPose.block<3, 3>(0, 0);
            toolTransform = origHandOri.transpose();
            // force calibration
            if (!dmpRunning)
            {
                forceOffset = (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
            }
 
            targetVel.setZero();
        }
        else
        {
            // communicate with DMP controller
            rtUpdateControlStruct();
            targetVel = rtGetControlStruct().targetTSVel;
            targetVel(2) = 0;
 
            // force detection
            filteredForce = (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * (forceSensor->force - forceOffset);
 
            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;
                }
            }
            Eigen::Matrix4f forceFrameInRoot = rtGetRobot()->getSensor(cfg->forceSensorName)->getRobotNode()->getPoseInRootFrame();
 
            filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
            float targetForce = user2rtData.getUpToDateReadBuffer().targetForce;
 
            //            Eigen::Matrix3f currentHandOri = currentPose.block<3, 3>(0, 0);
            //            Eigen::Matrix3f currentToolOri = toolTransform * currentHandOri;
            float desiredZVel = kpf * (targetForce - filteredForceInRoot(2));
            targetVel(2) -= desiredZVel;
            //            targetVel.block<3, 1>(0, 0) = currentToolOri * targetVel.block<3, 1>(0, 0);
 
 
            //            Eigen::Vector3f currentToolDir = currentToolOri * oriToolDir;
 
            for (int i = 3; i < 6; ++i)
            {
                targetVel(i) = 0;
            }
 
            // rotation changes
 
            //            if (filteredForceInRoot.norm() > fabs(cfg->minimumReactForce))
            //            {
            //                Eigen::Vector3f desiredToolDir = filteredForceInRoot / filteredForceInRoot.norm();
            //                currentToolDir = currentToolDir / currentToolDir.norm();
            //                Eigen::Vector3f axis = currentToolDir.cross(desiredToolDir);
            //                float angle = acosf(currentToolDir.dot(desiredToolDir));
 
            //                if (fabs(angle) < M_PI / 2)
            //                {
            //                    Eigen::AngleAxisf desiredToolRot(angle, axis);
            //                    Eigen::Matrix3f desiredRotMat = desiredToolRot * Eigen::Matrix3f::Identity();
 
            //                    targetPose.block<3, 3>(0, 0) = desiredRotMat * currentHandOri;
 
            //                    Eigen::Vector3f desiredRPY = VirtualRobot::MathTools::eigen3f2rpy(desiredRotMat);
            //                    Eigen::Vector3f checkedToolDir =  desiredRotMat * currentToolDir;
            //                    ARMARX_IMPORTANT << "axis: " << axis;
            //                    ARMARX_IMPORTANT << "angle: " << angle * 180 / 3.1415;
            //                    ARMARX_IMPORTANT << "desiredRotMat: " << desiredRotMat;
 
            //                    ARMARX_IMPORTANT << "desiredToolDir: " << desiredToolDir;
            //                    ARMARX_IMPORTANT << "currentToolDir: " << currentToolDir;
            //                    ARMARX_IMPORTANT << "checkedToolDir: " << checkedToolDir;
            //                }
 
            //            }
 
 
            // 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);
 
        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().targetPose = targetPose;
        debugRT.getWriteBuffer().currentPose = currentPose;
        debugRT.getWriteBuffer().filteredForce = filteredForceInRoot;
        debugRT.getWriteBuffer().targetVel = targetVel;
        debugRT.getWriteBuffer().adaptK = adaptK;
        debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
        debugRT.getWriteBuffer().currentTwist = currentTwist;
 
        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
 
        for (size_t ki = 0; ki < 3; ++ki)
        {
            jacobi.row(ki) = 0.001 * jacobi.row(ki); // 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;
 
        float manidist = 0;
        if (isManipulability)
        {
            nullspaceTorque = manipulabilityTracker->calculateVelocity(targetManipulability, kmani, true);
            manidist = manipulabilityTracker->computeDistance(targetManipulability);
        }
        else
        {
            nullspaceTorque = knull * (nullSpaceJointsVec - qpos) - dnull * 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));
                }
            }
        }
 
        debugRT.getWriteBuffer().manidist = manidist;
 
        debugRT.commitWrite();
 
 
    }
 
 
    void NJointPeriodicTSDMPCompliantController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
    {
        ARMARX_INFO << "Learning DMP ... ";
 
        LockGuardType guard {controllerMutex};
        dmpCtrl->learnDMPFromFiles(fileNames);
 
    }
 
    void NJointPeriodicTSDMPCompliantController::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 NJointPeriodicTSDMPCompliantController::setSpeed(Ice::Double times, const Ice::Current&)
    {
        LockGuardType guard {controllerMutex};
        dmpCtrl->setSpeed(times);
    }
 
 
    void NJointPeriodicTSDMPCompliantController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
    {
        LockGuardType guard {controllerMutex};
        dmpCtrl->setGoalPoseVec(goals);
    }
 
    void NJointPeriodicTSDMPCompliantController::setAmplitude(Ice::Double amp, const Ice::Current&)
    {
        LockGuardType guard {controllerMutex};
        dmpCtrl->setAmplitude(amp);
    }
 
    void NJointPeriodicTSDMPCompliantController::setTargetForceInRootFrame(float targetForce, const Ice::Current&)
    {
        LockGuardType guard {controlDataMutex};
        user2rtData.getWriteBuffer().targetForce = targetForce;
        user2rtData.commitWrite();
    }
 
    void NJointPeriodicTSDMPCompliantController::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 NJointPeriodicTSDMPCompliantController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, 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["filteredforce_x"] = new Variant(filteredForce(0));
        datafields["filteredforce_y"] = new Variant(filteredForce(1));
        datafields["filteredforce_z"] = new Variant(filteredForce(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));
 
        Eigen::VectorXf currentVel = debugRT.getUpToDateReadBuffer().currentTwist;
        datafields["currentVel_x"] = new Variant(currentVel(0));
        datafields["currentVel_y"] = new Variant(currentVel(1));
        datafields["currentVel_z"] = new Variant(currentVel(2));
 
        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["manidist"] = new Variant(debugRT.getUpToDateReadBuffer().manidist);
 
 
        //        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 NJointPeriodicTSDMPCompliantController::onDisconnectNJointController()
    {
        ARMARX_INFO << "stopped ...";
    }
 
 
 
}