DeprecatedNJointTaskSpaceImpedanceDMPController.cpp

Go to the documentation of this file.

#include "DeprecatedNJointTaskSpaceImpedanceDMPController.h"
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Nodes/Sensor.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.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<DeprecatedNJointTaskSpaceImpedanceDMPController>
        registrationControllerDeprecatedNJointTaskSpaceImpedanceDMPController(
            "DeprecatedNJointTaskSpaceImpedanceDMPController");
 
    DeprecatedNJointTaskSpaceImpedanceDMPController::
        DeprecatedNJointTaskSpaceImpedanceDMPController(
            const RobotUnitPtr& robotUnit,
            const armarx::NJointControllerConfigPtr& config,
            const VirtualRobot::RobotPtr&)
    {
        ARMARX_TRACE;
        ARMARX_INFO << "creating impedance dmp controller";
        cfg = DeprecatedNJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
        ARMARX_CHECK_NOT_NULL(cfg);
        useSynchronizedRtRobot();
        rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
        ARMARX_INFO << "1";
        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);
        };
        const SensorValueBase* svlf =
            robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
        forceSensor = svlf->asA<SensorValueForceTorque>();
 
        ARMARX_TRACE;
        forceOffset.setZero();
        filteredForce.setZero();
        filteredForceInRoot.setZero();
        ARMARX_INFO << cfg->forceThreshold;
        forceThreshold.reinitAllBuffers(cfg->forceThreshold);
        tcp = rns->getTCP();
        ik.reset(new VirtualRobot::DifferentialIK(
            rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
        ik->setDampedSvdLambda(0.0001);
 
        ARMARX_TRACE;
        numOfJoints = targets.size();
        // set DMP
        tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
        taskSpaceDMPConfig.DMPAmplitude = 1.0;
        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));
        finished = false;
 
        useNullSpaceJointDMP = cfg->useNullSpaceJointDMP;
        nullSpaceJointDMPPtr.reset(new DMP::UMIDMP(100));
 
        isNullSpaceJointDMPLearned = false;
 
        Eigen::VectorXf nullspaceValues(targets.size());
 
        ARMARX_CHECK_EQUAL(cfg->defaultNullSpaceJointValues.size(), targets.size());
 
        for (size_t i = 0; i < targets.size(); ++i)
        {
            nullspaceValues(i) = cfg->defaultNullSpaceJointValues.at(i);
        }
        defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
 
        ARMARX_TRACE;
        Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
        Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
        Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
        Eigen::Vector3f dori(cfg->Dori[0], cfg->Dori[1], cfg->Dori[2]);
        Eigen::VectorXf knull(targets.size());
        Eigen::VectorXf dnull(targets.size());
 
        ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
        ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
 
        for (size_t i = 0; i < targets.size(); ++i)
        {
            knull(i) = cfg->Knull.at(i);
            dnull(i) = cfg->Dnull.at(i);
        }
 
        CtrlParams initParams = {kpos, dpos, kori, dori, knull, dnull};
        ctrlParams.reinitAllBuffers(initParams);
 
        torqueLimit = cfg->torqueLimit;
        timeDuration = cfg->timeDuration;
 
        DeprecatedNJointTaskSpaceImpedanceDMPControllerInterfaceData initInterfaceData;
        initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
        interfaceData.reinitAllBuffers(initInterfaceData);
 
        DeprecatedNJointTaskSpaceImpedanceDMPControllerSensorData initControllerSensorData;
        initControllerSensorData.currentPose = Eigen::Matrix4f::Identity();
        initControllerSensorData.currentTime = 0;
        initControllerSensorData.deltaT = 0;
        initControllerSensorData.currentTwist.setZero();
        controllerSensorData.reinitAllBuffers(initControllerSensorData);
 
        firstRun = true;
        useForceStop = false;
 
        ARMARX_INFO << "Finished controller constructor ";
    }
 
    std::string
    DeprecatedNJointTaskSpaceImpedanceDMPController::getClassName(const Ice::Current&) const
    {
        return "DeprecatedNJointTaskSpaceImpedanceDMPController";
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::rtPreActivateController()
    {
        ARMARX_TRACE;
        DeprecatedNJointTaskSpaceImpedanceDMPControllerControlData initData;
        initData.targetPose = tcp->getPoseInRootFrame();
        initData.targetVel.resize(6);
        initData.targetVel.setZero();
        initData.desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
        reinitTripleBuffer(initData);
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::controllerRun()
    {
        if (!dmpCtrl)
        {
            return;
        }
 
        if (!controllerSensorData.updateReadBuffer())
        {
            return;
        }
 
 
        double deltaT = 0.001; //controllerSensorData.getReadBuffer().deltaT;
        Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
        Eigen::VectorXf currentTwist = controllerSensorData.getReadBuffer().currentTwist;
 
        if (!started)
        {
            LockGuardType guard{controlDataMutex};
            getWriterControlStruct().desiredNullSpaceJointValues =
                defaultNullSpaceJointValues.getUpToDateReadBuffer();
            //            getWriterControlStruct().targetVel.setZero(6);
            //            getWriterControlStruct().targetPose = currentPose;
            getWriterControlStruct().canVal = 1.0;
            getWriterControlStruct().mpcFactor = 0.0;
            writeControlStruct();
        }
        else
        {
            if (stopped)
            {
 
                LockGuardType guard{controlDataMutex};
                getWriterControlStruct().desiredNullSpaceJointValues =
                    defaultNullSpaceJointValues.getUpToDateReadBuffer();
                //                getWriterControlStruct().targetVel.setZero(6);
                //                getWriterControlStruct().targetPose = oldPose;
                getWriterControlStruct().canVal = dmpCtrl->canVal;
                getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
                writeControlStruct();
            }
            else
            {
                if (dmpCtrl->canVal < 1e-8)
                {
                    finished = true;
                    started = false;
                    LockGuardType guard{controlDataMutex};
                    getWriterControlStruct().targetVel.setZero();
                    writeControlStruct();
                    return;
                }
 
                dmpCtrl->flow(deltaT, currentPose, currentTwist);
 
                Eigen::VectorXf desiredNullSpaceJointValues(jointNames.size());
                if (useNullSpaceJointDMP && isNullSpaceJointDMPLearned)
                {
                    DMP::DVec targetJointState;
                    currentJointState = nullSpaceJointDMPPtr->calculateDirectlyVelocity(
                        currentJointState,
                        dmpCtrl->canVal / timeDuration,
                        deltaT / timeDuration,
                        targetJointState);
 
                    if (targetJointState.size() == jointNames.size())
                    {
                        for (size_t i = 0; i < targetJointState.size(); ++i)
                        {
                            desiredNullSpaceJointValues(i) = targetJointState[i];
                        }
                    }
                    else
                    {
                        desiredNullSpaceJointValues =
                            defaultNullSpaceJointValues.getUpToDateReadBuffer();
                    }
                }
                else
                {
                    desiredNullSpaceJointValues =
                        defaultNullSpaceJointValues.getUpToDateReadBuffer();
                }
 
                LockGuardType guard{controlDataMutex};
                getWriterControlStruct().desiredNullSpaceJointValues = desiredNullSpaceJointValues;
                getWriterControlStruct().targetVel = dmpCtrl->getTargetVelocity();
                getWriterControlStruct().targetPose = dmpCtrl->getTargetPoseMat();
                getWriterControlStruct().canVal = dmpCtrl->canVal;
                getWriterControlStruct().mpcFactor = dmpCtrl->debugData.mpcFactor;
 
                writeControlStruct();
            }
        }
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::rtRun(
        const IceUtil::Time& sensorValuesTimestamp,
        const IceUtil::Time& timeSinceLastIteration)
    {
 
        Eigen::Matrix4f currentPose = tcp->getPoseInRootFrame();
 
        double deltaT = timeSinceLastIteration.toSecondsDouble();
        Eigen::Matrix4f targetPose;
        Eigen::VectorXf targetVel;
        Eigen::VectorXf desiredNullSpaceJointValues;
        if (firstRun)
        {
            ARMARX_IMPORTANT << "impedance control first run";
            firstRun = false;
            targetPose = currentPose;
            stopPose = currentPose;
            targetVel.setZero(6);
            desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
        }
        else
        {
            if (!started)
            {
                targetPose = stopPose;
                targetVel.setZero(6);
                desiredNullSpaceJointValues = defaultNullSpaceJointValues.getUpToDateReadBuffer();
                forceOffset =
                    (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
                timeForCalibration = timeForCalibration + timeSinceLastIteration.toSecondsDouble();
            }
            else
            {
                targetPose = rtGetControlStruct().targetPose;
                targetVel = rtGetControlStruct().targetVel;
                desiredNullSpaceJointValues = rtGetControlStruct().desiredNullSpaceJointValues;
                stopPose = targetPose;
 
                if (useForceStop)
                {
                    /* handle force stop */
                    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;
 
                    for (size_t i = 0; i < 3; ++i)
                    {
                        if (fabs(filteredForceInRoot[i]) >
                            forceThreshold.getUpToDateReadBuffer()[i])
                        {
                            stopPose = currentPose;
                            targetVel.setZero(6);
                            desiredNullSpaceJointValues =
                                defaultNullSpaceJointValues.getUpToDateReadBuffer();
                            started = false;
                            break;
                        }
                    }
                }
            }
        }
 
 
        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;
        }
 
        Eigen::VectorXf currentTwist = jacobi * qvel;
 
        controllerSensorData.getWriteBuffer().currentPose = currentPose;
        controllerSensorData.getWriteBuffer().currentTwist = currentTwist;
        controllerSensorData.getWriteBuffer().deltaT = deltaT;
        controllerSensorData.getWriteBuffer().currentTime += deltaT;
        controllerSensorData.commitWrite();
 
        interfaceData.getWriteBuffer().currentTcpPose = currentPose;
        interfaceData.commitWrite();
 
        jacobi.block(0, 0, 3, numOfJoints) =
            0.001 * jacobi.block(0, 0, 3, numOfJoints); // convert mm to m
 
        Eigen::Vector3f kpos = ctrlParams.getUpToDateReadBuffer().kpos;
        Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
        Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
        Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
        Eigen::VectorXf knull = ctrlParams.getUpToDateReadBuffer().knull;
        Eigen::VectorXf dnull = ctrlParams.getUpToDateReadBuffer().dnull;
 
        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 tcpDesiredForce =
                0.001 * kpos.cwiseProduct(desiredPosition - currentTCPPosition) +
                dpos.cwiseProduct(targetTCPLinearVelocity - 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(desiredNullSpaceJointValues - 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 limit
        ARMARX_CHECK_EXPRESSION(!targets.empty());
        ARMARX_CHECK_LESS(targets.size(), 1000);
        for (size_t i = 0; i < targets.size(); ++i)
        {
            float desiredTorque = jointDesiredTorques(i);
 
            if (isnan(desiredTorque))
            {
                desiredTorque = 0;
            }
 
            desiredTorque = (desiredTorque > torqueLimit) ? torqueLimit : desiredTorque;
            desiredTorque = (desiredTorque < -torqueLimit) ? -torqueLimit : desiredTorque;
 
            debugOutputData.getWriteBuffer().desired_torques[jointNames[i]] =
                jointDesiredTorques(i);
            debugOutputData.getWriteBuffer().desired_nullspaceJoint[jointNames[i]] =
                desiredNullSpaceJointValues(i);
 
            targets.at(i)->torque = desiredTorque;
            if (!targets.at(i)->isValid())
            {
                ARMARX_INFO << deactivateSpam(1)
                            << "Torque controller target is invalid - setting to zero! set value: "
                            << targets.at(i)->torque;
                targets.at(i)->torque = 0.0f;
            }
        }
 
 
        debugOutputData.getWriteBuffer().forceDesired_x = jointControlWrench(0);
        debugOutputData.getWriteBuffer().forceDesired_y = jointControlWrench(1);
        debugOutputData.getWriteBuffer().forceDesired_z = jointControlWrench(2);
        debugOutputData.getWriteBuffer().forceDesired_rx = jointControlWrench(3);
        debugOutputData.getWriteBuffer().forceDesired_ry = jointControlWrench(4);
        debugOutputData.getWriteBuffer().forceDesired_rz = jointControlWrench(5);
 
        //        debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
        //        debugOutputData.getWriteBuffer().mpcfactor = rtGetControlStruct().mpcFactor;
 
        debugOutputData.getWriteBuffer().targetPose_x = targetPose(0, 3);
        debugOutputData.getWriteBuffer().targetPose_y = targetPose(1, 3);
        debugOutputData.getWriteBuffer().targetPose_z = targetPose(2, 3);
        VirtualRobot::MathTools::Quaternion targetQuat =
            VirtualRobot::MathTools::eigen4f2quat(targetPose);
        debugOutputData.getWriteBuffer().targetPose_qw = targetQuat.w;
        debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
        debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
        debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
        debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
 
        debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
        debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
        debugOutputData.getWriteBuffer().currentPose_z = currentPose(2, 3);
        VirtualRobot::MathTools::Quaternion currentQuat =
            VirtualRobot::MathTools::eigen4f2quat(currentPose);
        debugOutputData.getWriteBuffer().currentPose_qw = currentQuat.w;
        debugOutputData.getWriteBuffer().currentPose_qx = currentQuat.x;
        debugOutputData.getWriteBuffer().currentPose_qy = currentQuat.y;
        debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
        debugOutputData.getWriteBuffer().deltaT = deltaT;
 
        debugOutputData.getWriteBuffer().currentKpos_x = kpos.x();
        debugOutputData.getWriteBuffer().currentKpos_y = kpos.y();
        debugOutputData.getWriteBuffer().currentKpos_z = kpos.z();
        debugOutputData.getWriteBuffer().currentKori_x = kori.x();
        debugOutputData.getWriteBuffer().currentKori_y = kori.y();
        debugOutputData.getWriteBuffer().currentKori_z = kori.z();
        debugOutputData.getWriteBuffer().currentKnull_x = knull.x();
        debugOutputData.getWriteBuffer().currentKnull_y = knull.y();
        debugOutputData.getWriteBuffer().currentKnull_z = knull.z();
 
        debugOutputData.getWriteBuffer().currentDpos_x = dpos.x();
        debugOutputData.getWriteBuffer().currentDpos_y = dpos.y();
        debugOutputData.getWriteBuffer().currentDpos_z = dpos.z();
        debugOutputData.getWriteBuffer().currentDori_x = dori.x();
        debugOutputData.getWriteBuffer().currentDori_y = dori.y();
        debugOutputData.getWriteBuffer().currentDori_z = dori.z();
        debugOutputData.getWriteBuffer().currentDnull_x = dnull.x();
        debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
        debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
 
        debugOutputData.getWriteBuffer().filteredForceInRoot = filteredForceInRoot;
 
        debugOutputData.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::learnDMPFromFiles(
        const Ice::StringSeq& fileNames,
        const Ice::Current&)
    {
        dmpCtrl->learnDMPFromFiles(fileNames);
        ARMARX_INFO << "Learned DMP ... ";
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setViaPoints(Ice::Double u,
                                                                  const Ice::DoubleSeq& viapoint,
                                                                  const Ice::Current&)
    {
        LockGuardType guard(controllerMutex);
        ARMARX_INFO << "setting via points ";
        dmpCtrl->setViaPose(u, viapoint);
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setGoals(const Ice::DoubleSeq& goals,
                                                              const Ice::Current& ice)
    {
        dmpCtrl->setGoalPoseVec(goals);
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::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() != jointNames.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
    DeprecatedNJointTaskSpaceImpedanceDMPController::resetDMP(const Ice::Current&)
    {
        if (started)
        {
            ARMARX_INFO << "Cannot reset running DMP";
        }
        firstRun = true;
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::stopDMP(const Ice::Current&)
    {
        //        oldPose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
        started = false;
        stopped = true;
    }
 
    std::string
    DeprecatedNJointTaskSpaceImpedanceDMPController::getDMPAsString(const Ice::Current&)
    {
 
        return dmpCtrl->saveDMPToString();
    }
 
    Ice::DoubleSeq
    DeprecatedNJointTaskSpaceImpedanceDMPController::createDMPFromString(
        const std::string& dmpString,
        const Ice::Current&)
    {
        dmpCtrl->loadDMPFromString(dmpString);
        return dmpCtrl->dmpPtr->defaultGoal;
    }
 
    Ice::Double
    DeprecatedNJointTaskSpaceImpedanceDMPController::getCanVal(const Ice::Current&)
    {
        return dmpCtrl->canVal;
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::resumeDMP(const Ice::Current&)
    {
        stopped = false;
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setUseNullSpaceJointDMP(bool enable,
                                                                             const Ice::Current&)
    {
        useNullSpaceJointDMP = enable;
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::runDMPWithTime(const Ice::DoubleSeq& goals,
                                                                    Ice::Double timeDuration,
                                                                    const Ice::Current&)
    {
        dmpCtrl->canVal = timeDuration;
        dmpCtrl->config.motionTimeDuration = timeDuration;
 
        runDMP(goals);
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::runDMP(const Ice::DoubleSeq& goals,
                                                            const Ice::Current&)
    {
        firstRun = true;
        timeForCalibration = 0;
        started = false;
 
        while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
        {
            usleep(100);
        }
        while (!interfaceData.updateReadBuffer())
        {
            usleep(100);
        }
 
        while (!interfaceData.updateReadBuffer())
        {
            usleep(100);
        }
 
        Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
        ARMARX_IMPORTANT << "start to run dmp from: " << VAROUT(pose);
        LockGuardType guard{controllerMutex};
        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
        finished = false;
 
        if (isNullSpaceJointDMPLearned && useNullSpaceJointDMP)
        {
            ARMARX_INFO << "Using Null Space Joint DMP";
        }
        ARMARX_INFO << VAROUT(dmpCtrl->config.motionTimeDuration);
 
        started = true;
        stopped = false;
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::onPublish(
        const SensorAndControl&,
        const DebugDrawerInterfacePrx&,
        const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
        auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
        for (auto& pair : values)
        {
            datafields["torqueDesired_" + pair.first] = new Variant(pair.second);
        }
 
        auto values_null = debugOutputData.getUpToDateReadBuffer().desired_nullspaceJoint;
        for (auto& pair : values_null)
        {
            datafields["nullspaceDesired_" + pair.first] = new Variant(pair.second);
        }
 
        datafields["canVal"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentCanVal);
        datafields["mpcfactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcfactor);
        datafields["targetPose_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_x);
        datafields["targetPose_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_y);
        datafields["targetPose_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_z);
        datafields["targetPose_qw"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qw);
        datafields["targetPose_qx"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qx);
        datafields["targetPose_qy"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qy);
        datafields["targetPose_qz"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().targetPose_qz);
 
        datafields["currentPose_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_x);
        datafields["currentPose_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_y);
        datafields["currentPose_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_z);
        datafields["currentPose_qw"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qw);
        datafields["currentPose_qx"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qx);
        datafields["currentPose_qy"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
        datafields["currentPose_qz"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
 
        datafields["currentKpos_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_x);
        datafields["currentKpos_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_y);
        datafields["currentKpos_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_z);
        datafields["currentKori_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_x);
        datafields["currentKori_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_y);
        datafields["currentKori_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_z);
        datafields["currentKnull_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_x);
        datafields["currentKnull_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_y);
        datafields["currentKnull_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_z);
 
        datafields["currentDpos_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_x);
        datafields["currentDpos_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_y);
        datafields["currentDpos_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_z);
        datafields["currentDori_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_x);
        datafields["currentDori_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_y);
        datafields["currentDori_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_z);
        datafields["currentDnull_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_x);
        datafields["currentDnull_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_y);
        datafields["currentDnull_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_z);
 
        datafields["forceDesired_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
        datafields["forceDesired_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
        datafields["forceDesired_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
        datafields["forceDesired_rx"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rx);
        datafields["forceDesired_ry"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_ry);
        datafields["forceDesired_rz"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_rz);
 
        datafields["filteredForceInRoot_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[0]);
        datafields["filteredForceInRoot_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[1]);
        datafields["filteredForceInRoot_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().filteredForceInRoot[2]);
 
        datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
 
        std::string channelName = cfg->nodeSetName + "_TaskSpaceImpedanceControl";
        debugObs->setDebugChannel(channelName, datafields);
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::onInitNJointController()
    {
        ARMARX_INFO << "init ...";
        //        controllerTask = new PeriodicTask<DeprecatedNJointTaskSpaceImpedanceDMPController>(this, &DeprecatedNJointTaskSpaceImpedanceDMPController::controllerRun, 1);
        runTask("DeprecatedNJointTaskSpaceImpedanceDMPController",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            controllerRun();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::onDisconnectNJointController()
    {
        //        controllerTask->stop();
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setMPWeights(const DoubleSeqSeq& weights,
                                                                  const Ice::Current&)
    {
        dmpCtrl->setWeights(weights);
    }
 
    DoubleSeqSeq
    DeprecatedNJointTaskSpaceImpedanceDMPController::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
    DeprecatedNJointTaskSpaceImpedanceDMPController::removeAllViaPoints(const Ice::Current&)
    {
        LockGuardType guard{controllerMutex};
        ARMARX_INFO << "setting via points ";
        dmpCtrl->removeAllViaPoints();
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Eigen::Vector3f& kd,
                                                                         const Ice::Current&)
    {
        ARMARX_CHECK_EQUAL(kd.size(), 3);
        ARMARX_INFO << "set linear kd " << VAROUT(kd);
        LockGuardType guard(controllerMutex);
        ctrlParams.getWriteBuffer().dpos = kd;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Eigen::Vector3f& kp,
                                                                         const Ice::Current&)
    {
        ARMARX_CHECK_EQUAL(kp.size(), 3);
        ARMARX_INFO << "set linear kp " << VAROUT(kp);
        LockGuardType guard(controllerMutex);
        ctrlParams.getWriteBuffer().kpos = kp;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Eigen::Vector3f& kd,
                                                                          const Ice::Current&)
    {
        ARMARX_CHECK_EQUAL(kd.size(), 3);
        ARMARX_INFO << "set angular kd " << VAROUT(kd);
        LockGuardType guard(controllerMutex);
        ctrlParams.getWriteBuffer().dori = kd;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Eigen::Vector3f& kp,
                                                                          const Ice::Current&)
    {
        ARMARX_CHECK_EQUAL(kp.size(), 3);
        ARMARX_INFO << "set angular kp " << VAROUT(kp);
        LockGuardType guard(controllerMutex);
        ctrlParams.getWriteBuffer().kori = kp;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(
        const Eigen::VectorXf& kd,
        const Ice::Current&)
    {
        ARMARX_CHECK_EQUAL((std::size_t)kd.size(), targets.size());
        ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
        LockGuardType guard(controllerMutex);
        ctrlParams.getWriteBuffer().dnull = kd;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(
        const Eigen::VectorXf& kp,
        const Ice::Current&)
    {
        ARMARX_CHECK_EQUAL((std::size_t)kp.size(), targets.size());
        ARMARX_INFO << "set linear kp " << VAROUT(kp);
        LockGuardType guard(controllerMutex);
        ctrlParams.getWriteBuffer().knull = kp;
        ctrlParams.commitWrite();
    }
 
    void
    DeprecatedNJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(
        const Eigen::VectorXf& jointValues,
        const Ice::Current&)
    {
        ARMARX_CHECK_EQUAL((std::size_t)jointValues.size(), targets.size());
        defaultNullSpaceJointValues.getWriteBuffer() = jointValues;
        defaultNullSpaceJointValues.commitWrite();
    }
 
 
} // namespace armarx::control::deprecated_njoint_mp_controller::task_space