DeprecatedNJointTaskSpaceAdmittanceDMPController.cpp

Go to the documentation of this file.

#include "DeprecatedNJointTaskSpaceAdmittanceDMPController.h"
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Nodes/Sensor.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 <dmp/representation/dmp/umitsmp.h>
 
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
    NJointControllerRegistration<DeprecatedNJointTaskSpaceAdmittanceDMPController>
        registrationControllerDeprecatedNJointTaskSpaceAdmittanceDMPController(
            "DeprecatedNJointTaskSpaceAdmittanceDMPController");
 
    DeprecatedNJointTaskSpaceAdmittanceDMPController::
        DeprecatedNJointTaskSpaceAdmittanceDMPController(
            const RobotUnitPtr& robotUnit,
            const armarx::NJointControllerConfigPtr& config,
            const VirtualRobot::RobotPtr&)
    {
        ARMARX_INFO << "creating task-space admittance dmp controller";
 
        useSynchronizedRtRobot();
        cfg = DeprecatedNJointTaskSpaceAdmittanceDMPControllerConfigPtr::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
 
        /// setup kinematics
        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();
            jointNames.push_back(jointName);
 
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
 
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No velocitySensor available for " << jointName;
            }
            if (!positionSensor)
            {
                ARMARX_WARNING << "No positionSensor available for " << jointName;
            }
 
            velocitySensors.push_back(velocitySensor);
            positionSensors.push_back(positionSensor);
        };
 
        tcp = rns->getTCP();
        ik.reset(new VirtualRobot::DifferentialIK(
            rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
        ik->setDampedSvdLambda(0.0001);
        numOfJoints = targets.size();
        qvel_filtered.setZero(targets.size());
        torqueLimit = cfg->jointTorqueLimit;
 
        /// setup force sensor and filters
        const SensorValueBase* svlf =
            robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
        forceSensor = svlf->asA<SensorValueForceTorque>();
 
        float tcpMass = 0.0f;
        Eigen::Vector3f tcpCoMInForceSensorFrame;
        tcpCoMInForceSensorFrame.setZero();
 
        enableTCPGravityCompensation.store(cfg->enableTCPGravityCompensation);
        ARMARX_INFO << VAROUT(enableTCPGravityCompensation);
        if (enableTCPGravityCompensation.load())
        {
            tcpMass = cfg->tcpMass;
            tcpCoMInForceSensorFrame = cfg->tcpCoMInForceSensorFrame;
        }
 
        forceOffset.setZero();
        torqueOffset.setZero();
        filteredForce.setZero();
        filteredTorque.setZero();
        filteredForceInRoot.setZero();
        filteredTorqueInRoot.setZero();
 
        /// set MP
        /// - task space
        tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
        taskSpaceDMPConfig.DMPAmplitude = 1.0;
        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
        taskSpaceDMPConfig.phaseStopParas.Kpos = 0;
        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
        taskSpaceDMPConfig.phaseStopParas.Kori = 0;
        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
 
        dmpCtrl.reset(
            new tsvmp::TaskSpaceDMPController("DMPController", taskSpaceDMPConfig, false));
        kinematic_chain = cfg->nodeSetName;
 
        /// - null space
        useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
        nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
        isNullSpaceJointDMPLearned = false;
        ARMARX_CHECK_EQUAL(cfg->targetNullSpaceJointValues.size(), targets.size());
 
        /// Admittance vairables
        virtualAcc.setZero();
        virtualVel.setZero();
        virtualPose = Eigen::Matrix4f::Identity();
 
        /// initialize control parameters and buffers
        /// - control parameter
        ARMARX_CHECK_EQUAL(cfg->Knull.size(), static_cast<int>(targets.size()));
        ARMARX_CHECK_EQUAL(cfg->Dnull.size(), static_cast<int>(targets.size()));
        CtrlParams initParams = {cfg->kpImpedance,
                                 cfg->kdImpedance,
                                 cfg->kpAdmittance,
                                 cfg->kdAdmittance,
                                 cfg->kmAdmittance,
                                 cfg->Knull,
                                 cfg->Dnull,
                                 Eigen::Vector3f::Zero(),
                                 Eigen::Vector3f::Zero(),
                                 tcpMass,
                                 tcpCoMInForceSensorFrame};
        ctrlParams.reinitAllBuffers(initParams);
 
        ARMARX_INFO << "Finished controller constructor ";
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::onInitNJointController()
    {
        ARMARX_INFO << "init ...";
        /// initialize control parameters and buffers
        /// - control target
        DeprecatedNJointTaskSpaceAdmittanceDMPControllerControlData initData;
        initData.targetTSPose = tcp->getPoseInRootFrame();
        initData.targetTSVel.setZero();
        initData.targetNullSpaceJointValues = cfg->targetNullSpaceJointValues;
        reinitTripleBuffer(initData);
 
        /// - rt to interface
        RT2InterfaceData rt2InterfaceData;
        rt2InterfaceData.currentTcpPose = tcp->getPoseInRootFrame();
        rt2InterfaceBuffer.reinitAllBuffers(rt2InterfaceData);
 
        /// - rt to mp
        RT2MPData rt2MPData;
        rt2MPData.currentPose = tcp->getPoseInRootFrame();
        rt2MPData.currentTime = 0;
        rt2MPData.deltaT = 0;
        rt2MPData.currentTwist.setZero();
        rt2MPBuffer.reinitAllBuffers(rt2MPData);
 
        /// start mp thread
        runTask("DeprecatedNJointTaskSpaceAdmittanceDMPController",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            controllerRun();
                        }
                        c.waitForCycleDuration();
                    }
                });
 
        ARMARX_IMPORTANT << "started controller ";
    }
 
    std::string
    DeprecatedNJointTaskSpaceAdmittanceDMPController::getClassName(const Ice::Current&) const
    {
        return "DeprecatedNJointTaskSpaceAdmittanceDMPController";
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::rtPreActivateController()
    {
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::controllerRun()
    {
        if (!dmpCtrl || !rt2MPBuffer.updateReadBuffer())
        {
            return;
        }
 
        LockGuardType guard(mpMutex);
        double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
        Eigen::Matrix4f currentPose = rt2MPBuffer.getReadBuffer().currentPose;
        Eigen::VectorXf currentTwist = rt2MPBuffer.getReadBuffer().currentTwist;
 
        if (mpRunning.load() && dmpCtrl->canVal > 1e-8)
        {
            dmpCtrl->flow(deltaT, currentPose, currentTwist);
 
            Eigen::VectorXf targetNullSpaceJointValues = cfg->targetNullSpaceJointValues;
            if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
            {
                DMP::DVec targetJointState;
                currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(
                    currentJointState,
                    dmpCtrl->canVal / cfg->timeDuration,
                    deltaT / cfg->timeDuration,
                    targetJointState);
 
                if (targetJointState.size() == targets.size())
                {
                    for (size_t i = 0; i < targetJointState.size(); ++i)
                    {
                        targetNullSpaceJointValues(i) = targetJointState[i];
                    }
                }
            }
 
            LockGuardType guard{controlDataMutex};
            getWriterControlStruct().targetNullSpaceJointValues = targetNullSpaceJointValues;
            getWriterControlStruct().targetTSVel = dmpCtrl->getTargetVelocity();
            getWriterControlStruct().targetTSPose = dmpCtrl->getTargetPoseMat();
            getWriterControlStruct().canVal = dmpCtrl->canVal;
            getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
            writeControlStruct();
        }
        else
        {
            mpRunning.store(false);
            LockGuardType guard{controlDataMutex};
            getWriterControlStruct().targetTSVel.setZero();
            writeControlStruct();
        }
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::rtRun(
        const IceUtil::Time& sensorValuesTimestamp,
        const IceUtil::Time& timeSinceLastIteration)
    {
        /// ---------------------------- get current kinematics ---------------------------------------------
        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
        Eigen::MatrixXf jacobi =
            ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
 
        Eigen::VectorXf qpos(positionSensors.size());
        Eigen::VectorXf qvel(velocitySensors.size());
        for (size_t i = 0; i < positionSensors.size(); ++i)
        {
            qpos(i) = positionSensors[i]->position;
            qvel(i) = velocitySensors[i]->velocity;
        }
        qvel_filtered = (1 - cfg->qvelFilter) * qvel_filtered + cfg->qvelFilter * qvel;
        Eigen::VectorXf currentTwist = jacobi * qvel_filtered;
        jacobi.block(0, 0, 3, numOfJoints) =
            0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
 
        double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        /// write rt buffer
        {
            rt2MPBuffer.getWriteBuffer().currentPose = currentPose;
            rt2MPBuffer.getWriteBuffer().currentTwist = currentTwist;
            rt2MPBuffer.getWriteBuffer().deltaT = deltaT;
            rt2MPBuffer.getWriteBuffer().currentTime += deltaT;
            rt2MPBuffer.commitWrite();
 
            rt2InterfaceBuffer.getWriteBuffer().currentTcpPose = currentPose;
            rt2InterfaceBuffer.commitWrite();
        }
 
        /// ---------------------------- update control parameters ---------------------------------------------
        Eigen::Vector6f kpImpedance = ctrlParams.getUpToDateReadBuffer().kpImpedance;
        Eigen::Vector6f kdImpedance = ctrlParams.getUpToDateReadBuffer().kdImpedance;
 
        Eigen::Vector6f kpAdmittance = ctrlParams.getUpToDateReadBuffer().kpAdmittance;
        Eigen::Vector6f kdAdmittance = ctrlParams.getUpToDateReadBuffer().kdAdmittance;
        Eigen::Vector6f kmAdmittance = Eigen::Vector6f::Zero();
 
        Eigen::VectorXf knull = ctrlParams.getUpToDateReadBuffer().knull;
        Eigen::VectorXf dnull = ctrlParams.getUpToDateReadBuffer().dnull;
 
        float tcpMass = ctrlParams.getUpToDateReadBuffer().tcpMass;
        Eigen::Vector3f tcpCoMInFTFrame =
            ctrlParams.getUpToDateReadBuffer().tcpCoMInForceSensorFrame;
 
        /// ---------------------------- get current force torque value ---------------------------------------------
        Eigen::Vector3f forceBaseline = ctrlParams.getUpToDateReadBuffer().forceBaseline;
        Eigen::Vector3f torqueBaseline = ctrlParams.getUpToDateReadBuffer().torqueBaseline;
        Eigen::Vector6f forceTorque;
        Eigen::Vector6f tcpGravityCompensation;
        forceTorque.setZero();
        tcpGravityCompensation.setZero();
 
        /// ---------------------------- get control targets ---------------------------------------------
        Eigen::Matrix4f targetPose;
        Eigen::Vector6f targetVel = Eigen::Vector6f::Zero();
        Eigen::VectorXf targetNullSpaceJointValues =
            rtGetControlStruct().targetNullSpaceJointValues;
 
        if (rtFirstRun.load())
        {
            rtReady.store(false);
            rtFirstRun.store(false);
            timeForCalibration = 0;
            forceOffset.setZero();
            torqueOffset.setZero();
 
            targetPose = currentPose;
            previousTargetPose = currentPose;
            virtualPose = currentPose;
 
            Eigen::Matrix4f forceFrameInRoot =
                rtGetRobot()->getSensor(cfg->forceSensorName)->getRobotNode()->getPoseInRootFrame();
            forceFrameInTCP.block<3, 3>(0, 0) =
                currentPose.block<3, 3>(0, 0).transpose() * forceFrameInRoot.block<3, 3>(0, 0);
            tcpCoMInTCPFrame = forceFrameInTCP.block<3, 3>(0, 0) * tcpCoMInFTFrame;
            ARMARX_IMPORTANT << "impedance control first run with\n"
                             << VAROUT(targetPose) << "\nTCP CoM Vec (in TCP frame): \n"
                             << VAROUT(tcpCoMInTCPFrame);
        }
        else
        {
            if (enableTCPGravityCompensation.load())
            {
                tcpGravityCompensation = getTCPGravityCompensation(currentPose, tcpMass);
            }
            if (!rtReady.load())
            {
                targetPose = previousTargetPose;
                forceOffset =
                    (1 - cfg->forceFilter) * forceOffset +
                    cfg->forceFilter * (forceSensor->force - tcpGravityCompensation.head<3>());
                torqueOffset =
                    (1 - cfg->forceFilter) * torqueOffset +
                    cfg->forceFilter * (forceSensor->torque - tcpGravityCompensation.tail<3>());
                timeForCalibration = timeForCalibration + deltaT;
                if (timeForCalibration > cfg->waitTimeForCalibration)
                {
                    /// rt is ready only if the FT sensor is calibrated, and the rt2interface buffer is updated (see above)
                    ARMARX_IMPORTANT << "FT calibration done, RT is ready! \n"
                                     << VAROUT(forceOffset) << "\n"
                                     << VAROUT(torqueOffset) << "\n"
                                     << VAROUT(tcpGravityCompensation);
                    rtReady.store(true);
                }
            }
            else
            {
                /// only when the ft sensor is calibrated can we
                kmAdmittance = ctrlParams.getUpToDateReadBuffer().kmAdmittance;
 
                targetPose = rtGetControlStruct().targetTSPose;
                targetVel = rtGetControlStruct().targetTSVel;
                if ((previousTargetPose.block<3, 1>(0, 3) - targetPose.block<3, 1>(0, 3)).norm() >
                    100.0f)
                {
                    ARMARX_WARNING << "new target \n"
                                   << VAROUT(targetPose) << "\nis too far away from\n"
                                   << VAROUT(previousTargetPose);
                    targetPose = previousTargetPose;
                }
                previousTargetPose = targetPose;
                forceTorque =
                    getFilteredForceTorque(forceBaseline, torqueBaseline, tcpGravityCompensation);
            }
        }
 
        /// ---------------------------- admittance control ---------------------------------------------
        /// calculate pose error between the virtual pose and the target pose
        Eigen::VectorXf poseError(6);
        poseError.head<3>() = virtualPose.block<3, 1>(0, 3) - targetPose.block<3, 1>(0, 3);
        Eigen::Matrix3f objDiffMat =
            virtualPose.block<3, 3>(0, 0) * targetPose.block<3, 3>(0, 0).transpose();
        poseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
 
        /// admittance control law and Euler Integration -> virtual pose
        Eigen::Vector6f acc = Eigen::Vector6f::Zero();
        Eigen::Vector6f vel = Eigen::Vector6f::Zero();
        acc = kmAdmittance.cwiseProduct(forceTorque) - kpAdmittance.cwiseProduct(poseError) -
              kdAdmittance.cwiseProduct(virtualVel);
        vel = virtualVel + 0.5 * deltaT * (acc + virtualAcc);
        Eigen::VectorXf deltaPose = 0.5 * deltaT * (vel + virtualVel);
        virtualAcc = acc;
        virtualVel = vel;
        virtualPose.block<3, 1>(0, 3) += deltaPose.head<3>();
        Eigen::Matrix3f deltaPoseMat =
            VirtualRobot::MathTools::rpy2eigen3f(deltaPose(3), deltaPose(4), deltaPose(5));
        virtualPose.block<3, 3>(0, 0) = deltaPoseMat * virtualPose.block<3, 3>(0, 0);
 
        /// ----------------------------- Impedance control ---------------------------------------------
        /// calculate pose error between virtual pose and current pose
        /// !!! This is very important: you have to keep postion and orientation both
        /// with UI unit (meter, radian) to calculate impedance force.
        Eigen::Vector6f poseErrorImp;
        Eigen::Matrix3f diffMat =
            virtualPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).transpose();
        poseErrorImp.head<3>() =
            0.001 * (virtualPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3));
        currentTwist.head<3>() *= 0.001;
        poseErrorImp.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
        Eigen::Vector6f forceImpedance =
            kpImpedance.cwiseProduct(poseErrorImp) - kdImpedance.cwiseProduct(currentTwist);
 
        /// ----------------------------- Nullspace PD Control --------------------------------------------------
        Eigen::VectorXf nullspaceTorque =
            knull.cwiseProduct(targetNullSpaceJointValues - qpos) - dnull.cwiseProduct(qvel);
 
        /// ----------------------------- Map TS target force to JS --------------------------------------------------
        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(targets.size(), targets.size());
        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(jacobi.transpose(), 2.0);
        Eigen::VectorXf jointDesiredTorques = jacobi.transpose() * forceImpedance +
                                              (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
 
        /// ----------------------------- Send torque command to joint device --------------------------------------------------
        ARMARX_CHECK_EXPRESSION(!targets.empty());
        for (size_t i = 0; i < targets.size(); ++i)
        {
            targets.at(i)->torque = jointDesiredTorques(i);
            if (!targets.at(i)->isValid() || isnan(targets.at(i)->torque))
            {
                targets.at(i)->torque = 0.0f;
            }
            else
            {
                if (fabs(targets.at(i)->torque) > fabs(torqueLimit))
                {
                    targets.at(i)->torque =
                        fabs(torqueLimit) * (targets.at(i)->torque / fabs(targets.at(i)->torque));
                }
            }
 
            debugRTBuffer.getWriteBuffer().desired_torques[jointNames[i]] = targets.at(i)->torque;
            debugRTBuffer.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] =
                targetNullSpaceJointValues(i);
        }
 
 
        // debug information
        {
            debugRTBuffer.getWriteBuffer().forceDesired_x = forceImpedance(0);
            debugRTBuffer.getWriteBuffer().forceDesired_y = forceImpedance(1);
            debugRTBuffer.getWriteBuffer().forceDesired_z = forceImpedance(2);
            debugRTBuffer.getWriteBuffer().forceDesired_rx = forceImpedance(3);
            debugRTBuffer.getWriteBuffer().forceDesired_ry = forceImpedance(4);
            debugRTBuffer.getWriteBuffer().forceDesired_rz = forceImpedance(5);
 
            debugRTBuffer.getWriteBuffer().ft_InRoot_x = filteredForce(0);
            debugRTBuffer.getWriteBuffer().ft_InRoot_y = filteredForce(1);
            debugRTBuffer.getWriteBuffer().ft_InRoot_z = filteredForce(2);
            debugRTBuffer.getWriteBuffer().ft_InRoot_rx = filteredTorque(0);
            debugRTBuffer.getWriteBuffer().ft_InRoot_ry = filteredTorque(1);
            debugRTBuffer.getWriteBuffer().ft_InRoot_rz = filteredTorque(2);
 
            debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_x = forceTorque(0);
            debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_y = forceTorque(1);
            debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_z = forceTorque(2);
            debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_rx = forceTorque(3);
            debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_ry = forceTorque(4);
            debugRTBuffer.getWriteBuffer().ft_FilteredInRoot_rz = forceTorque(5);
 
            debugRTBuffer.getWriteBuffer().targetPose_x = targetPose(0, 3);
            debugRTBuffer.getWriteBuffer().targetPose_y = targetPose(1, 3);
            debugRTBuffer.getWriteBuffer().targetPose_z = targetPose(2, 3);
            VirtualRobot::MathTools::Quaternion targetQuat =
                VirtualRobot::MathTools::eigen4f2quat(targetPose);
            debugRTBuffer.getWriteBuffer().targetPose_qw = targetQuat.w;
            debugRTBuffer.getWriteBuffer().targetPose_qx = targetQuat.x;
            debugRTBuffer.getWriteBuffer().targetPose_qy = targetQuat.y;
            debugRTBuffer.getWriteBuffer().targetPose_qz = targetQuat.z;
 
            debugRTBuffer.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
            debugRTBuffer.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
            debugRTBuffer.getWriteBuffer().virtualPose_z = virtualPose(2, 3);
            VirtualRobot::MathTools::Quaternion virtualQuat =
                VirtualRobot::MathTools::eigen4f2quat(virtualPose);
            debugRTBuffer.getWriteBuffer().virtualPose_qw = virtualQuat.w;
            debugRTBuffer.getWriteBuffer().virtualPose_qx = virtualQuat.x;
            debugRTBuffer.getWriteBuffer().virtualPose_qy = virtualQuat.y;
            debugRTBuffer.getWriteBuffer().virtualPose_qz = virtualQuat.z;
 
            debugRTBuffer.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
 
            debugRTBuffer.getWriteBuffer().currentPose_x = currentPose(0, 3);
            debugRTBuffer.getWriteBuffer().currentPose_y = currentPose(1, 3);
            debugRTBuffer.getWriteBuffer().currentPose_z = currentPose(2, 3);
            VirtualRobot::MathTools::Quaternion currentQuat =
                VirtualRobot::MathTools::eigen4f2quat(currentPose);
            debugRTBuffer.getWriteBuffer().currentPose_qw = currentQuat.w;
            debugRTBuffer.getWriteBuffer().currentPose_qx = currentQuat.x;
            debugRTBuffer.getWriteBuffer().currentPose_qy = currentQuat.y;
            debugRTBuffer.getWriteBuffer().currentPose_qz = currentQuat.z;
            debugRTBuffer.getWriteBuffer().deltaT = deltaT;
 
            debugRTBuffer.getWriteBuffer().currentKpos_x = kpImpedance(0);
            debugRTBuffer.getWriteBuffer().currentKpos_y = kpImpedance(1);
            debugRTBuffer.getWriteBuffer().currentKpos_z = kpImpedance(2);
            debugRTBuffer.getWriteBuffer().currentKori_x = kpImpedance(3);
            debugRTBuffer.getWriteBuffer().currentKori_y = kpImpedance(4);
            debugRTBuffer.getWriteBuffer().currentKori_z = kpImpedance(5);
            debugRTBuffer.getWriteBuffer().currentKnull_x = knull.x();
            debugRTBuffer.getWriteBuffer().currentKnull_y = knull.y();
            debugRTBuffer.getWriteBuffer().currentKnull_z = knull.z();
 
            debugRTBuffer.getWriteBuffer().currentDpos_x = kdImpedance(0);
            debugRTBuffer.getWriteBuffer().currentDpos_y = kdImpedance(1);
            debugRTBuffer.getWriteBuffer().currentDpos_z = kdImpedance(2);
            debugRTBuffer.getWriteBuffer().currentDori_x = kdImpedance(3);
            debugRTBuffer.getWriteBuffer().currentDori_y = kdImpedance(4);
            debugRTBuffer.getWriteBuffer().currentDori_z = kdImpedance(5);
            debugRTBuffer.getWriteBuffer().currentDnull_x = dnull.x();
            debugRTBuffer.getWriteBuffer().currentDnull_y = dnull.y();
            debugRTBuffer.getWriteBuffer().currentDnull_z = dnull.z();
 
            debugRTBuffer.getWriteBuffer().kmAdmittance = kmAdmittance;
            debugRTBuffer.getWriteBuffer().gravityCompensation = tcpGravityCompensation;
            debugRTBuffer.getWriteBuffer().forceOffset = forceOffset;
            debugRTBuffer.getWriteBuffer().torqueOffset = torqueOffset;
            debugRTBuffer.getWriteBuffer().forceBaseline = forceBaseline;
            debugRTBuffer.getWriteBuffer().torqueBaseline = torqueBaseline;
 
            debugRTBuffer.commitWrite();
        }
    }
 
    Eigen::Vector6f
    DeprecatedNJointTaskSpaceAdmittanceDMPController::getTCPGravityCompensation(
        Eigen::Matrix4f& currentPose,
        float tcpMass)
    {
        // static compensation
        Eigen::Vector3f gravity;
        gravity << 0.0, 0.0, -9.8;
        Eigen::Vector3f localGravity = currentPose.block<3, 3>(0, 0).transpose() * gravity;
        Eigen::Vector3f localForceVec = tcpMass * localGravity;
        Eigen::Vector3f localTorqueVec = tcpCoMInTCPFrame.cross(localForceVec);
        //        ARMARX_INFO << VAROUT(localForceVec) << VAROUT(localTorqueVec) << VAROUT(tcpMass) << VAROUT(enableTCPGravityCompensation.load());
 
        Eigen::Vector6f tcpGravityCompensation;
        tcpGravityCompensation << localForceVec, localTorqueVec;
        return tcpGravityCompensation;
    }
 
    Eigen::Vector6f
    DeprecatedNJointTaskSpaceAdmittanceDMPController::getFilteredForceTorque(
        Eigen::Vector3f& forceBaseline,
        Eigen::Vector3f& torqueBaseline,
        Eigen::Vector6f& handCompensatedFT)
    {
        filteredForce =
            (1 - cfg->forceFilter) * filteredForce + cfg->forceFilter * forceSensor->force;
        filteredTorque =
            (1 - cfg->forceFilter) * filteredTorque + cfg->forceFilter * forceSensor->torque;
 
        Eigen::Matrix4f forceFrameInRoot =
            rtGetRobot()->getSensor(cfg->forceSensorName)->getRobotNode()->getPoseInRootFrame();
        filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) *
                                  (filteredForce - forceOffset - handCompensatedFT.head<3>()) -
                              forceBaseline;
        filteredTorqueInRoot = forceFrameInRoot.block<3, 3>(0, 0) *
                                   (filteredTorque - torqueOffset - handCompensatedFT.tail<3>()) -
                               torqueBaseline;
 
        for (size_t i = 0; i < 3; ++i)
        {
            if (fabs(filteredForceInRoot(i)) > cfg->forceDeadZone)
            {
                filteredForceInRoot(i) -=
                    (filteredForceInRoot(i) / fabs(filteredForceInRoot(i))) * cfg->forceDeadZone;
            }
            else
            {
                filteredForceInRoot(i) = 0;
            }
 
            if (fabs(filteredTorqueInRoot(i)) > cfg->torqueDeadZone)
            {
                filteredTorqueInRoot(i) -=
                    (filteredTorqueInRoot(i) / fabs(filteredTorqueInRoot(i))) * cfg->torqueDeadZone;
            }
            else
            {
                filteredTorqueInRoot(i) = 0;
            }
        }
 
        Eigen::Vector6f forceTorque;
        forceTorque << filteredForceInRoot, filteredTorqueInRoot;
        return forceTorque;
    }
 
    std::string
    DeprecatedNJointTaskSpaceAdmittanceDMPController::getKinematicChainName(const Ice::Current&)
    {
        return kinematic_chain;
    }
 
    //    RTController::ControllerInfo DeprecatedNJointTaskSpaceAdmittanceDMPController::getControllerInfo(const Ice::Current& ice)
    //    {
    //        return info;
    //    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::learnDMPFromFiles(
        const Ice::StringSeq& fileNames,
        const Ice::Current&)
    {
        dmpCtrl->learnDMPFromFiles(fileNames);
        ARMARX_INFO << "Learned DMP ... ";
    }
 
    bool
    DeprecatedNJointTaskSpaceAdmittanceDMPController::isFinished(const Ice::Current&)
    {
        return !mpRunning.load();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setViaPoints(Ice::Double u,
                                                                   const Ice::DoubleSeq& viapoint,
                                                                   const Ice::Current&)
    {
        LockGuardType guard(mpMutex);
        ARMARX_INFO << "setting via points ";
        dmpCtrl->setViaPose(u, viapoint);
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setGoals(const Ice::DoubleSeq& goals,
                                                               const Ice::Current& ice)
    {
        dmpCtrl->setGoalPoseVec(goals);
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setStartAndGoals(const Ice::DoubleSeq& starts,
                                                                       const Ice::DoubleSeq& goals,
                                                                       const Ice::Current& ice)
    {
        LockGuardType guard(mpMutex);
        dmpCtrl->removeAllViaPoints();
        dmpCtrl->setViaPose(1.0, starts);
        dmpCtrl->setViaPose(1e-8, goals);
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::learnJointDMPFromFiles(
        const std::string& fileName,
        const Ice::FloatSeq& currentJVS,
        const Ice::Current&)
    {
        DMP::Vec<DMP::SampledTrajectoryV2> trajs;
        DMP::DVec ratios;
        DMP::SampledTrajectoryV2 traj;
        traj.readFromCSVFile(fileName);
        traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
        if (traj.dim() != targets.size())
        {
            isNullSpaceJointDMPLearned = false;
            return;
        }
 
        DMP::DVec goal;
        goal.resize(traj.dim());
        currentJointState.resize(traj.dim());
 
        for (size_t i = 0; i < goal.size(); ++i)
        {
            goal.at(i) = traj.rbegin()->getPosition(i);
            currentJointState.at(i).pos = currentJVS.at(i);
            currentJointState.at(i).vel = 0;
        }
 
        trajs.push_back(traj);
        nullSpaceJointDMPPtr->learnFromTrajectories(trajs);
 
        // prepare exeuction of joint dmp
        nullSpaceJointDMPPtr->prepareExecution(goal, currentJointState, 1.0, 1.0);
        ARMARX_INFO << "prepared nullspace joint dmp";
        isNullSpaceJointDMPLearned = true;
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::resetDMP(const Ice::Current&)
    {
        //        if (started)
        if (mpRunning.load())
        {
            ARMARX_INFO << "Cannot reset running DMP";
        }
        rtFirstRun = true;
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::stopDMP(const Ice::Current&)
    {
        mpRunning.store(false);
        //        oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
        //        started = false;
        //        stopped = true;
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::pauseDMP(const Ice::Current&)
    {
        dmpCtrl->pauseController();
    }
 
    std::string
    DeprecatedNJointTaskSpaceAdmittanceDMPController::getDMPAsString(const Ice::Current&)
    {
 
        return dmpCtrl->saveDMPToString();
    }
 
    Ice::DoubleSeq
    DeprecatedNJointTaskSpaceAdmittanceDMPController::createDMPFromString(
        const std::string& dmpString,
        const Ice::Current&)
    {
        dmpCtrl->loadDMPFromString(dmpString);
        return dmpCtrl->dmpPtr->defaultGoal;
    }
 
    Ice::Double
    DeprecatedNJointTaskSpaceAdmittanceDMPController::getCanVal(const Ice::Current&)
    {
        return dmpCtrl->canVal;
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::resumeDMP(const Ice::Current&)
    {
        dmpCtrl->resumeController();
        //        stopped = false;
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setUseNullSpaceJointDMP(bool enable,
                                                                              const Ice::Current&)
    {
        useNullSpaceJointDMP = enable;
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals,
                                                                     Ice::Double timeDuration,
                                                                     const Ice::Current&)
    {
        dmpCtrl->canVal = timeDuration;
        dmpCtrl->config.motionTimeDuration = timeDuration;
 
        runDMP(goals);
    }
 
    Ice::Double
    DeprecatedNJointTaskSpaceAdmittanceDMPController::getVirtualTime(const Ice::Current&)
    {
        return dmpCtrl->canVal;
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::runDMP(const Ice::DoubleSeq& goals,
                                                             const Ice::Current&)
    {
        rtFirstRun.store(true);
        while (rtFirstRun.load() || !rtReady.load() || !rt2InterfaceBuffer.updateReadBuffer())
        {
            usleep(100);
        }
 
        Eigen::Matrix4f pose = rt2InterfaceBuffer.getUpToDateReadBuffer().currentTcpPose;
 
        LockGuardType guard{mpMutex};
        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
 
        if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
        {
            ARMARX_INFO << "Using Null Space Joint DMP";
        }
        ARMARX_INFO << VAROUT(dmpCtrl->config.motionTimeDuration);
        ARMARX_INFO << VAROUT(dmpCtrl->dmpPtr->getViaPoints().size());
 
        mpRunning.store(true);
        dmpCtrl->resumeController();
        ARMARX_INFO << "start mp ...";
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::onPublish(
        const SensorAndControl&,
        const DebugDrawerInterfacePrx&,
        const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
        auto values = debugRTBuffer.getUpToDateReadBuffer().desired_torques;
        for (auto& pair : values)
        {
            datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
        }
 
        auto values_null = debugRTBuffer.getUpToDateReadBuffer().desired_nullspaceJoint;
        for (auto& pair : values_null)
        {
            datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
        }
 
        datafields["canVal"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().currentCanVal);
        datafields["mpcfactor"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().mpcfactor);
        datafields["targetPose_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_x);
        datafields["targetPose_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_y);
        datafields["targetPose_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_z);
        datafields["targetPose_qw"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_qw);
        datafields["targetPose_qx"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_qx);
        datafields["targetPose_qy"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_qy);
        datafields["targetPose_qz"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().targetPose_qz);
 
        datafields["virtualPose_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_x);
        datafields["virtualPose_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_y);
        datafields["virtualPose_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_z);
        datafields["virtualPose_qw"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_qw);
        datafields["virtualPose_qx"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_qx);
        datafields["virtualPose_qy"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_qy);
        datafields["virtualPose_qz"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().virtualPose_qz);
 
        datafields["currentPose_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_x);
        datafields["currentPose_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_y);
        datafields["currentPose_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_z);
        datafields["currentPose_qw"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_qw);
        datafields["currentPose_qx"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_qx);
        datafields["currentPose_qy"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_qy);
        datafields["currentPose_qz"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentPose_qz);
 
        datafields["currentKpos_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKpos_x);
        datafields["currentKpos_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKpos_y);
        datafields["currentKpos_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKpos_z);
        datafields["currentKori_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKori_x);
        datafields["currentKori_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKori_y);
        datafields["currentKori_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKori_z);
        datafields["currentKnull_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKnull_x);
        datafields["currentKnull_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKnull_y);
        datafields["currentKnull_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentKnull_z);
 
        datafields["currentDpos_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDpos_x);
        datafields["currentDpos_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDpos_y);
        datafields["currentDpos_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDpos_z);
        datafields["currentDori_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDori_x);
        datafields["currentDori_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDori_y);
        datafields["currentDori_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDori_z);
        datafields["currentDnull_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDnull_x);
        datafields["currentDnull_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDnull_y);
        datafields["currentDnull_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().currentDnull_z);
 
        datafields["forceDesired_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_x);
        datafields["forceDesired_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_y);
        datafields["forceDesired_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_z);
        datafields["forceDesired_rx"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_rx);
        datafields["forceDesired_ry"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_ry);
        datafields["forceDesired_rz"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceDesired_rz);
 
        datafields["ft_InRoot_x"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_x);
        datafields["ft_InRoot_y"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_y);
        datafields["ft_InRoot_z"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_z);
        datafields["ft_InRoot_rx"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_rx);
        datafields["ft_InRoot_ry"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_ry);
        datafields["ft_InRoot_rz"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_InRoot_rz);
 
        datafields["ft_FilteredInRoot_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_x);
        datafields["ft_FilteredInRoot_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_y);
        datafields["ft_FilteredInRoot_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_z);
        datafields["ft_FilteredInRoot_rx"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_rx);
        datafields["ft_FilteredInRoot_ry"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_ry);
        datafields["ft_FilteredInRoot_rz"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().ft_FilteredInRoot_rz);
 
        datafields["deltaT"] = new Variant(debugRTBuffer.getUpToDateReadBuffer().deltaT);
 
        datafields["kmAdmittance_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(0));
        datafields["kmAdmittance_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(1));
        datafields["kmAdmittance_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(2));
        datafields["kmAdmittance_rx"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(3));
        datafields["kmAdmittance_ry"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(4));
        datafields["kmAdmittance_rz"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().kmAdmittance(5));
 
        datafields["gravityCompensation_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(0));
        datafields["gravityCompensation_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(1));
        datafields["gravityCompensation_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(2));
        datafields["gravityCompensation_rx"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(3));
        datafields["gravityCompensation_ry"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(4));
        datafields["gravityCompensation_rz"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().gravityCompensation(5));
 
 
        datafields["ft_offset_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceOffset(0));
        datafields["ft_offset_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceOffset(1));
        datafields["ft_offset_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceOffset(2));
        datafields["ft_offset_rx"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueOffset(0));
        datafields["ft_offset_ry"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueOffset(1));
        datafields["ft_offset_rz"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueOffset(2));
 
        datafields["ft_Baseline_x"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceBaseline(0));
        datafields["ft_Baseline_y"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceBaseline(1));
        datafields["ft_Baseline_z"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().forceBaseline(2));
        datafields["ft_Baseline_rx"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueBaseline(0));
        datafields["ft_Baseline_ry"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueBaseline(1));
        datafields["ft_Baseline_rz"] =
            new Variant(debugRTBuffer.getUpToDateReadBuffer().torqueBaseline(2));
 
        std::string channelName = cfg->nodeSetName + "_TaskSpaceAdmittanceControl";
        debugObs->setDebugChannel(channelName, datafields);
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::onDisconnectNJointController()
    {
        //        controllerTask->stop();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setMPWeights(const DoubleSeqSeq& weights,
                                                                   const Ice::Current&)
    {
        dmpCtrl->setWeights(weights);
    }
 
    DoubleSeqSeq
    DeprecatedNJointTaskSpaceAdmittanceDMPController::getMPWeights(const Ice::Current&)
    {
        DMP::DVec2d res = dmpCtrl->getWeights();
        DoubleSeqSeq resvec;
        for (size_t i = 0; i < res.size(); ++i)
        {
            std::vector<double> cvec;
            for (size_t j = 0; j < res[i].size(); ++j)
            {
                cvec.push_back(res[i][j]);
            }
            resvec.push_back(cvec);
        }
 
        return resvec;
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::removeAllViaPoints(const Ice::Current&)
    {
        ARMARX_INFO << "remove all via points";
        LockGuardType guard{mpMutex};
        dmpCtrl->removeAllViaPoints();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setKpImpedance(
        const Eigen::Vector6f& kpImpedance,
        const Ice::Current&)
    {
        ctrlParams.getWriteBuffer().kpImpedance = kpImpedance;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setKdImpedance(
        const Eigen::Vector6f& kdImpedance,
        const Ice::Current&)
    {
        ctrlParams.getWriteBuffer().kdImpedance = kdImpedance;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setKpAdmittance(
        const Eigen::Vector6f& kpAdmittance,
        const Ice::Current&)
    {
        ctrlParams.getWriteBuffer().kpAdmittance = kpAdmittance;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setKdAdmittance(
        const Eigen::Vector6f& kdAdmittance,
        const Ice::Current&)
    {
        ctrlParams.getWriteBuffer().kdAdmittance = kdAdmittance;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setKmAdmittance(
        const Eigen::Vector6f& kmAdmittance,
        const Ice::Current&)
    {
        ctrlParams.getWriteBuffer().kmAdmittance = kmAdmittance;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setTCPMass(Ice::Float mass,
                                                                 const Ice::Current&)
    {
        ctrlParams.getWriteBuffer().tcpMass = mass;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setTCPCoMInFTFrame(const Eigen::Vector3f& com,
                                                                         const Ice::Current&)
    {
        ctrlParams.getWriteBuffer().tcpCoMInForceSensorFrame = com;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setNullspaceVelocityKd(
        const Eigen::VectorXf& kd,
        const Ice::Current&)
    {
        ARMARX_CHECK_EQUAL(kd.size(), targets.size());
        ARMARX_INFO << "set nullspace damping\n" << VAROUT(kd);
        ctrlParams.getWriteBuffer().dnull = kd;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setNullspaceVelocityKp(
        const Eigen::VectorXf& kp,
        const Ice::Current&)
    {
        ARMARX_CHECK_EQUAL(kp.size(), targets.size());
        ARMARX_INFO << "set nullspace stiffness\n" << VAROUT(kp);
        ctrlParams.getWriteBuffer().knull = kp;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setTargetNullSpaceJointValues(
        const Eigen::VectorXf& jointValues,
        const Ice::Current&)
    {
        ARMARX_CHECK_EQUAL(jointValues.size(), targets.size());
        getWriterControlStruct().targetNullSpaceJointValues = jointValues;
        writeControlStruct();
    }
 
    void
    DeprecatedNJointTaskSpaceAdmittanceDMPController::setPotentialForceBaseline(
        const Eigen::Vector3f& force,
        const Eigen::Vector3f& torque,
        const Ice::Current&)
    {
        ctrlParams.getWriteBuffer().forceBaseline = force;
        ctrlParams.getWriteBuffer().torqueBaseline = torque;
        ctrlParams.commitWrite();
    }
 
 
} // namespace armarx::control::deprecated_njoint_mp_controller::task_space