NJointTSDMPController.cpp

Go to the documentation of this file.

#include "NJointTSDMPController.h"
 
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/IK/DifferentialIK.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
 
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
    NJointControllerRegistration<NJointTSDMPController>
        registrationControllerNJointTSDMPController("NJointTSDMPController");
 
    NJointTSDMPController::NJointTSDMPController(const RobotUnitPtr&,
                                                 const armarx::NJointControllerConfigPtr& config,
                                                 const VirtualRobot::RobotPtr&)
    {
        useSynchronizedRtRobot();
        cfg = NJointTaskSpaceDMPControllerConfigPtr::dynamicCast(config);
        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
        jointNames = rns->getNodeNames();
        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::Velocity1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
 
            const SensorValue1DoFActuatorTorque* torqueSensor =
                sv->asA<SensorValue1DoFActuatorTorque>();
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFGravityTorque* gravityTorqueSensor =
                sv->asA<SensorValue1DoFGravityTorque>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
            if (!torqueSensor)
            {
                ARMARX_WARNING << "No Torque sensor available for " << jointName;
            }
            if (!gravityTorqueSensor)
            {
                ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
            }
 
            torqueSensors.push_back(torqueSensor);
            gravityTorqueSensors.push_back(gravityTorqueSensor);
            velocitySensors.push_back(velocitySensor);
            positionSensors.push_back(positionSensor);
        };
 
        tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
        refFrame = (cfg->frameName.empty()) ? rns->getRobot()->getRootNode()
                                            : rtGetRobot()->getRobotNode(cfg->frameName);
        ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName;
 
        // set tcp controller
        tcpController.reset(new CartesianVelocityController(rns, tcp));
        nodeSetName = cfg->nodeSetName;
        torquePIDs.resize(tcpController->rns->getSize(), pidController());
 
        ik.reset(new VirtualRobot::DifferentialIK(
            rns, refFrame, VirtualRobot::JacobiProvider::eSVDDamped));
 
 
        finished = false;
        tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
        taskSpaceDMPConfig.DMPStyle = cfg->dmpStyle;
        taskSpaceDMPConfig.DMPAmplitude = cfg->dmpAmplitude;
        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
        taskSpaceDMPConfig.phaseStopParas.Kpos = cfg->phaseKpPos;
        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
        taskSpaceDMPConfig.phaseStopParas.Kori = cfg->phaseKpOri;
        taskSpaceDMPConfig.phaseStopParas.Dori = 0;
        taskSpaceDMPConfig.phaseStopParas.mm2radi = cfg->posToOriRatio;
        taskSpaceDMPConfig.phaseStopParas.maxValue = cfg->phaseL;
        taskSpaceDMPConfig.phaseStopParas.slop = cfg->phaseK;
 
        dmpCtrl.reset(new tsvmp::TaskSpaceDMPController("default", taskSpaceDMPConfig, false));
 
        // initialize tcp position and orientation
 
 
        RTToControllerData initSensorData;
        initSensorData.deltaT = 0;
        initSensorData.currentTime = 0;
        initSensorData.currentPose.setZero();
        initSensorData.currentTwist.setZero();
        rt2CtrlData.reinitAllBuffers(initSensorData);
 
        targetVels.setZero(6);
        NJointTSDMPControllerControlData initData;
        initData.targetTSVel.setZero(6);
        initData.targetPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
        initData.nullspaceJointVelocities.resize(tcpController->rns->getSize(), 0);
        initData.torqueKp.resize(tcpController->rns->getSize(), 0);
        initData.torqueKd.resize(tcpController->rns->getSize(), 0);
        initData.mode = ModeFromIce(cfg->mode);
        reinitTripleBuffer(initData);
 
        debugName = cfg->debugName;
 
        KpF = cfg->Kp_LinearVel;
        KoF = cfg->Kp_AngularVel;
        DpF = cfg->Kd_LinearVel;
        DoF = cfg->Kd_AngularVel;
 
        filtered_qvel.setZero(targets.size());
        vel_filter_factor = cfg->vel_filter;
 
        filtered_position.setZero(3);
        pos_filter_factor = cfg->pos_filter;
 
        //        jlhigh = rns->getNode("..")->getJointLimitHi();
        //        jllow = rns->getNode("")->getJointLimitLo();
        firstRun = true;
 
        jointLowLimits.setZero(targets.size());
        jointHighLimits.setZero(targets.size());
        for (size_t i = 0; i < rns->getSize(); i++)
        {
            VirtualRobot::RobotNodePtr rn = rns->getAllRobotNodes().at(i);
 
            jointLowLimits(i) = rn->getJointLimitLo();
            jointHighLimits(i) = rn->getJointLimitHi();
        }
 
        started = false;
 
        RTToUserData initInterfaceData;
        initInterfaceData.currentTcpPose = Eigen::Matrix4f::Identity();
        rt2UserData.reinitAllBuffers(initInterfaceData);
    }
 
    std::string
    NJointTSDMPController::getClassName(const Ice::Current&) const
    {
        return "NJointTSDMPController";
    }
 
    void
    NJointTSDMPController::controllerRun()
    {
        if (!started)
        {
            return;
        }
 
        if (!rt2CtrlData.updateReadBuffer() || !dmpCtrl)
        {
            return;
        }
 
        double deltaT = rt2CtrlData.getReadBuffer().deltaT;
        Eigen::Matrix4f currentPose = rt2CtrlData.getReadBuffer().currentPose;
        Eigen::VectorXf currentTwist = rt2CtrlData.getReadBuffer().currentTwist;
 
        LockGuardType guard{controllerMutex};
        dmpCtrl->flow(deltaT, currentPose, currentTwist);
 
        if (dmpCtrl->canVal < 1e-8)
        {
            finished = true;
        }
        targetVels = dmpCtrl->getTargetVelocity();
        targetPose = dmpCtrl->getTargetPoseMat();
        std::vector<double> targetState = dmpCtrl->getTargetPose();
 
        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().dmpTargets["dmp_x"] = targetState[0];
        debugOutputData.getWriteBuffer().dmpTargets["dmp_y"] = targetState[1];
        debugOutputData.getWriteBuffer().dmpTargets["dmp_z"] = targetState[2];
        debugOutputData.getWriteBuffer().dmpTargets["dmp_qw"] = targetState[3];
        debugOutputData.getWriteBuffer().dmpTargets["dmp_qx"] = targetState[4];
        debugOutputData.getWriteBuffer().dmpTargets["dmp_qy"] = targetState[5];
        debugOutputData.getWriteBuffer().dmpTargets["dmp_qz"] = targetState[6];
        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;
        getWriterControlStruct().targetPose = targetPose;
        writeControlStruct();
    }
 
    Eigen::VectorXf
    NJointTSDMPController::calcIK(const Eigen::VectorXf& cartesianVel,
                                  const Eigen::VectorXf& nullspaceVel,
                                  VirtualRobot::IKSolver::CartesianSelection mode)
    {
        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, mode);
 
        Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
 
        Eigen::MatrixXf nullspace = lu_decomp.kernel();
        Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
        for (int i = 0; i < nullspace.cols(); i++)
        {
            float squaredNorm = nullspace.col(i).squaredNorm();
            // Prevent division by zero
            if (squaredNorm > 1.0e-32f)
            {
                nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) /
                       nullspace.col(i).squaredNorm();
            }
        }
 
        Eigen::MatrixXf inv =
            ik->computePseudoInverseJacobianMatrix(jacobi, ik->getJacobiRegularization(mode));
        //        ARMARX_INFO << "inv: " << inv;
        Eigen::VectorXf jointVel = inv * cartesianVel;
        //        jointVel += nsv;
        return jointVel;
    }
 
    void
    NJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                 const IceUtil::Time& timeSinceLastIteration)
    {
        Eigen::Matrix4f currentPose = refFrame->toLocalCoordinateSystem(tcp->getGlobalPose());
        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
        rt2UserData.commitWrite();
 
        if (firstRun)
        {
            filtered_position = currentPose.block<3, 1>(0, 3);
 
            firstRun = false;
            for (size_t i = 0; i < targets.size(); ++i)
            {
                targets.at(i)->velocity = 0;
            }
            return;
        }
        else
        {
            filtered_position = (1 - pos_filter_factor) * filtered_position +
                                pos_filter_factor * currentPose.block<3, 1>(0, 3);
        }
 
        double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        Eigen::MatrixXf jacobi =
            ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
 
        Eigen::VectorXf qvel(velocitySensors.size());
        for (size_t i = 0; i < velocitySensors.size(); ++i)
        {
            qvel(i) = velocitySensors[i]->velocity;
        }
 
        filtered_qvel = (1 - vel_filter_factor) * filtered_qvel + vel_filter_factor * qvel;
        Eigen::VectorXf tcptwist = jacobi * filtered_qvel;
 
        rt2CtrlData.getWriteBuffer().currentPose = currentPose;
        rt2CtrlData.getWriteBuffer().currentTwist = tcptwist;
        rt2CtrlData.getWriteBuffer().deltaT = deltaT;
        rt2CtrlData.getWriteBuffer().currentTime += deltaT;
        rt2CtrlData.commitWrite();
 
        rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
        rt2UserData.commitWrite();
 
        Eigen::VectorXf targetVel = rtGetControlStruct().targetTSVel;
        Eigen::Matrix4f targetPose = rtGetControlStruct().targetPose;
 
        Eigen::VectorXf jointTargetVelocities = Eigen::VectorXf::Zero(targets.size());
        if (started)
        {
            //            targetVel = rtGetControlStruct().targetTSVel;
            //            targetPose = rtGetControlStruct().targetPose;
 
            Eigen::Matrix3f diffMat =
                targetPose.block<3, 3>(0, 0) * currentPose.block<3, 3>(0, 0).inverse();
            Eigen::Vector3f errorRPY = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
 
            Eigen::Vector6f rtTargetVel;
            rtTargetVel.block<3, 1>(0, 0) =
                KpF * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) +
                DpF * (-tcptwist.block<3, 1>(0, 0));
            //        rtTargetVel.block<3, 1>(0, 0) = KpF * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) + DpF * (targetVel.block<3, 1>(0, 0) - tcptwist.block<3, 1>(0, 0));
            rtTargetVel.block<3, 1>(3, 0) =
                KoF * errorRPY + DoF * (targetVel.block<3, 1>(3, 0) - tcptwist.block<3, 1>(3, 0));
            //        rtTargetVel = targetVel;
 
 
            float normLinearVelocity = rtTargetVel.block<3, 1>(0, 0).norm();
            if (normLinearVelocity > cfg->maxLinearVel)
            {
                rtTargetVel.block<3, 1>(0, 0) =
                    cfg->maxLinearVel * rtTargetVel.block<3, 1>(0, 0) / normLinearVelocity;
            }
 
            float normAngularVelocity = rtTargetVel.block<3, 1>(3, 0).norm();
            if (normAngularVelocity > cfg->maxAngularVel)
            {
                rtTargetVel.block<3, 1>(3, 0) =
                    cfg->maxAngularVel * rtTargetVel.block<3, 1>(3, 0) / normAngularVelocity;
            }
 
 
            // cartesian vel controller
            //            Eigen::Vector6f x;
            //            for (size_t i = 0; i < 6; i++)
            //            {
            //                x(i) = rtTargetVel(i);
            //            }
 
            Eigen::VectorXf jnv = Eigen::VectorXf::Zero(tcpController->rns->getSize());
            float jointLimitAvoidanceKp = rtGetControlStruct().avoidJointLimitsKp;
            if (jointLimitAvoidanceKp > 0)
            {
                jnv += jointLimitAvoidanceKp * tcpController->calculateJointLimitAvoidance();
            }
            for (size_t i = 0; i < tcpController->rns->getSize(); i++)
            {
                jnv(i) += rtGetControlStruct().nullspaceJointVelocities.at(i);
            }
 
            //            jointTargetVelocities = tcpController->calculate(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
            jointTargetVelocities =
                calcIK(rtTargetVel, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
            // Eigen::VectorXf jointTargetVelocities = tcpController->calculate(x, jnv, VirtualRobot::IKSolver::CartesianSelection::All);
            ARMARX_CHECK_EXPRESSION(!targets.empty());
            ARMARX_CHECK_LESS(targets.size(), 1000);
        }
 
        for (size_t i = 0; i < targets.size(); ++i)
        {
            targets.at(i)->velocity = jointTargetVelocities(i);
 
            if (!targets.at(i)->isValid() || fabs(targets.at(i)->velocity) > cfg->maxJointVelocity)
            {
                targets.at(i)->velocity = 0.0f;
            }
        }
        rtDebugData.getWriteBuffer().targetJointVels = jointTargetVelocities;
        rtDebugData.commitWrite();
    }
 
    void
    NJointTSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames, const Ice::Current&)
    {
        ARMARX_INFO << "Learning DMP ... ";
 
        LockGuardType guard{controllerMutex};
        dmpCtrl->learnDMPFromFiles(fileNames);
    }
 
    void
    NJointTSDMPController::setSpeed(Ice::Double times, const Ice::Current&)
    {
        LockGuardType guard{controllerMutex};
        dmpCtrl->setSpeed(times);
    }
 
    void
    NJointTSDMPController::setViaPoints(Ice::Double u,
                                        const Ice::DoubleSeq& viapoint,
                                        const Ice::Current&)
    {
        LockGuardType guard{controllerMutex};
        dmpCtrl->setViaPose(u, viapoint);
    }
 
    void
    NJointTSDMPController::setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        for (size_t i = 0; i < tcpController->rns->getSize(); i++)
        {
            getWriterControlStruct().torqueKp.at(i) =
                torqueKp.at(tcpController->rns->getNode(i)->getName());
        }
        writeControlStruct();
    }
 
    void
    NJointTSDMPController::setNullspaceJointVelocities(
        const StringFloatDictionary& nullspaceJointVelocities,
        const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        for (size_t i = 0; i < tcpController->rns->getSize(); i++)
        {
            getWriterControlStruct().nullspaceJointVelocities.at(i) =
                nullspaceJointVelocities.at(tcpController->rns->getNode(i)->getName());
        }
        writeControlStruct();
    }
 
    void
    NJointTSDMPController::setControllerTarget(
        Ice::Float avoidJointLimitsKp,
        NJointTaskSpaceDMPControllerMode::CartesianSelection mode,
        const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
        getWriterControlStruct().mode = ModeFromIce(mode);
        writeControlStruct();
    }
 
    void
    NJointTSDMPController::removeAllViaPoints(const Ice::Current&)
    {
        LockGuardType guard{controllerMutex};
        ARMARX_INFO << "setting via points ";
        dmpCtrl->removeAllViaPoints();
    }
 
    void
    NJointTSDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
    {
        LockGuardType guard{controllerMutex};
        dmpCtrl->setGoalPoseVec(goals);
    }
 
    void
    NJointTSDMPController::pauseDMP(const Ice::Current&)
    {
        dmpCtrl->pauseController();
    }
 
    void
    NJointTSDMPController::resumeDMP(const Ice::Current&)
    {
        dmpCtrl->resumeController();
    }
 
    void
    NJointTSDMPController::resetDMP(const Ice::Current&)
    {
        if (started)
        {
            ARMARX_INFO << "Cannot reset running DMP";
        }
        firstRun = true;
    }
 
    void
    NJointTSDMPController::stopDMP(const Ice::Current&)
    {
        started = false;
    }
 
    std::string
    NJointTSDMPController::getDMPAsString(const Ice::Current&)
    {
 
        return dmpCtrl->saveDMPToString();
    }
 
    std::vector<double>
    NJointTSDMPController::createDMPFromString(const std::string& dmpString, const Ice::Current&)
    {
        dmpCtrl->loadDMPFromString(dmpString);
        return dmpCtrl->dmpPtr->defaultGoal;
    }
 
    VirtualRobot::IKSolver::CartesianSelection
    NJointTSDMPController::ModeFromIce(
        const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
    {
        if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::ePosition)
        {
            return VirtualRobot::IKSolver::CartesianSelection::Position;
        }
        if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eOrientation)
        {
            return VirtualRobot::IKSolver::CartesianSelection::Orientation;
        }
        if (mode == NJointTaskSpaceDMPControllerMode::CartesianSelection::eAll)
        {
            return VirtualRobot::IKSolver::CartesianSelection::All;
        }
        ARMARX_ERROR_S << "invalid mode " << mode;
        return (VirtualRobot::IKSolver::CartesianSelection)0;
    }
 
    void
    NJointTSDMPController::runDMP(const Ice::DoubleSeq& goals, double tau, const Ice::Current&)
    {
        ARMARX_INFO << "------dmp controller: " << VAROUT(goals);
        firstRun = true;
        while (firstRun)
        {
            usleep(100);
        }
        while (!rt2UserData.updateReadBuffer())
        {
            usleep(100);
        }
 
        Eigen::Matrix4f pose = rt2UserData.getReadBuffer().currentTcpPose;
 
        LockGuardType guard{controllerMutex};
        //        Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
        finished = false;
 
        ARMARX_INFO << "run DMP";
        started = true;
    }
 
    void
    NJointTSDMPController::runDMPWithTime(const Ice::DoubleSeq& goals,
                                          Ice::Double timeDuration,
                                          const Ice::Current&)
    {
        firstRun = true;
        while (firstRun)
        {
            usleep(100);
        }
        while (!rt2UserData.updateReadBuffer())
        {
            usleep(100);
        }
 
        Eigen::Matrix4f pose = rt2UserData.getReadBuffer().currentTcpPose;
 
        LockGuardType guard{controllerMutex};
        dmpCtrl->config.motionTimeDuration = timeDuration;
        dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
 
        finished = false;
        started = true;
    }
 
    void
    NJointTSDMPController::rtPreActivateController()
    {
    }
 
    void
    NJointTSDMPController::rtPostDeactivateController()
    {
    }
 
    void
    NJointTSDMPController::onPublish(const SensorAndControl&,
                                     const DebugDrawerInterfacePrx&,
                                     const DebugObserverInterfacePrx& debugObs)
    {
        std::string datafieldName = debugName;
        StringVariantBaseMap datafields;
        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
        for (auto& pair : values)
        {
            datafieldName = pair.first + "_" + debugName;
            datafields[datafieldName] = new Variant(pair.second);
        }
 
        auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
        for (auto& pair : dmpTargets)
        {
            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);
 
 
        Eigen::VectorXf targetJoints = rtDebugData.getUpToDateReadBuffer().targetJointVels;
        for (int i = 0; i < targetJoints.size(); ++i)
        {
            datafieldName = jointNames[i] + "_velocity";
            datafields[datafieldName] = new Variant(targetJoints[i]);
        }
 
        datafieldName = "DMPController_" + debugName;
        debugObs->setDebugChannel(datafieldName, datafields);
    }
 
    void
    NJointTSDMPController::onInitNJointController()
    {
        ARMARX_INFO << "init ...";
        started = false;
        runTask("NJointTSDMPController",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            controllerRun();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    NJointTSDMPController::onDisconnectNJointController()
    {
        ARMARX_INFO << "stopped ...";
    }
 
    void
    NJointTSDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
    {
        dmpCtrl->setWeights(weights);
    }
 
    DoubleSeqSeq
    NJointTSDMPController::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
    NJointTSDMPController::setLinearVelocityKd(Ice::Float kd, const Ice::Current&)
    {
        DpF = kd;
    }
 
    void
    NJointTSDMPController::setLinearVelocityKp(Ice::Float kp, const Ice::Current&)
    {
        KpF = kp;
    }
 
    void
    NJointTSDMPController::setAngularVelocityKd(Ice::Float kd, const Ice::Current&)
    {
        DoF = kd;
    }
 
    void
    NJointTSDMPController::setAngularVelocityKp(Ice::Float kp, const Ice::Current&)
    {
        KoF = kp;
    }
 
} // namespace armarx::control::deprecated_njoint_mp_controller::task_space