NJointCCDMPController.cpp

Go to the documentation of this file.

#include "NJointCCDMPController.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 <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/libraries/core/CartesianVelocityController.h>
 
namespace armarx::control::deprecated_njoint_mp_controller::task_space
{
    NJointControllerRegistration<NJointCCDMPController>
        registrationControllerNJointCCDMPController("NJointCCDMPController");
 
    NJointCCDMPController::NJointCCDMPController(const RobotUnitPtr&,
                                                 const armarx::NJointControllerConfigPtr& config,
                                                 const VirtualRobot::RobotPtr&)
    {
        useSynchronizedRtRobot();
        cfg = NJointCCDMPControllerConfigPtr::dynamicCast(config);
        ARMARX_CHECK_NOT_NULL(cfg);
        ARMARX_CHECK_GREATER_EQUAL(cfg->dmpNum, 0);
        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
 
        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
        for (size_t i = 0; i < rns->getSize(); ++i)
        {
            std::string jointName = rns->getNode(i)->getName();
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::VelocityTorque);
            const SensorValueBase* sv = useSensorValue(jointName);
            targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
 
            const SensorValue1DoFActuatorTorque* torqueSensor =
                sv->asA<SensorValue1DoFActuatorTorque>();
            const SensorValue1DoFGravityTorque* gravityTorqueSensor =
                sv->asA<SensorValue1DoFGravityTorque>();
            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);
        };
 
        tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
        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, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
 
        // set DMP
        isDisturbance = false;
 
        dmpPtrList.resize(cfg->dmpNum);
        canVals.resize(cfg->dmpNum);
        timeDurations = cfg->timeDurations;
        dmpTypes = cfg->dmpTypes;
        for (size_t i = 0; i < dmpPtrList.size(); ++i)
        {
            dmpPtrList[i].reset(new DMP::UMITSMP(cfg->kernelSize, 2, cfg->tau));
            canVals[i] = timeDurations[i];
        }
        finished = false;
 
        phaseL = cfg->phaseL;
        phaseK = cfg->phaseK;
        phaseDist0 = cfg->phaseDist0;
        phaseDist1 = cfg->phaseDist1;
        phaseKpPos = cfg->phaseKpPos;
        phaseKpOri = cfg->phaseKpOri;
 
        posToOriRatio = cfg->posToOriRatio;
        tau = cfg->tau;
 
        // initialize tcp position and orientation
        Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
        tcpPosition(0) = pose(0, 3);
        tcpPosition(1) = pose(1, 3);
        tcpPosition(2) = pose(2, 3);
        VirtualRobot::MathTools::Quaternion tcpQ = VirtualRobot::MathTools::eigen4f2quat(pose);
        tcpOrientation.w() = tcpQ.w;
        tcpOrientation.x() = tcpQ.x;
        tcpOrientation.y() = tcpQ.y;
        tcpOrientation.z() = tcpQ.z;
 
        NJointCCDMPControllerSensorData initSensorData;
        initSensorData.deltaT = 0;
        initSensorData.currentTime = 0;
        initSensorData.currentPose = pose;
        controllerSensorData.reinitAllBuffers(initSensorData);
 
 
        currentStates.resize(cfg->dmpNum);
        targetSubStates.resize(cfg->dmpNum);
        targetState.resize(7);
 
        targetVels.resize(6);
        targetVels.setZero();
        NJointCCDMPControllerControlData initData;
        initData.targetTSVel.resize(6);
        initData.targetTSVel.setZero();
        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);
 
        learnedDMP.clear();
 
        amplitudes = cfg->amplitudes;
    }
 
    std::string
    NJointCCDMPController::getClassName(const Ice::Current&) const
    {
        return "NJointCCDMPController";
    }
 
    void
    NJointCCDMPController::controllerRun()
    {
        if (!controllerSensorData.updateReadBuffer())
        {
            return;
        }
 
        double deltaT = controllerSensorData.getReadBuffer().deltaT;
 
        Eigen::Matrix4f currentPose = controllerSensorData.getReadBuffer().currentPose;
        Eigen::Vector3f currentPosition;
        currentPosition << currentPose(0, 3), currentPose(1, 3), currentPose(2, 3);
        double posError = 0;
        for (size_t i = 0; i < 3; ++i)
        {
            posError += pow(currentPosition(i) - targetState[i], 2);
        }
        posError = sqrt(posError);
 
 
        VirtualRobot::MathTools::Quaternion cQuat =
            VirtualRobot::MathTools::eigen4f2quat(currentPose);
        Eigen::Quaterniond currentQ;
        Eigen::Quaterniond targetQ;
        currentQ.w() = cQuat.w;
        currentQ.x() = cQuat.x;
        currentQ.y() = cQuat.y;
        currentQ.z() = cQuat.z;
        targetQ.w() = targetState[3];
        targetQ.x() = targetState[4];
        targetQ.y() = targetState[5];
        targetQ.z() = targetState[6];
 
        double oriError = targetQ.angularDistance(currentQ);
        if (oriError > M_PI)
        {
            oriError = 2 * M_PI - oriError;
        }
 
        double error = posError + posToOriRatio * oriError;
 
        double phaseDist;
        if (isDisturbance)
        {
            phaseDist = phaseDist1;
        }
        else
        {
            phaseDist = phaseDist0;
        }
 
        double phaseStop = phaseL / (1 + exp(-phaseK * (error - phaseDist)));
        double mpcFactor = 1 - (phaseStop / phaseL);
 
 
        if (mpcFactor < 0.1)
        {
            isDisturbance = true;
        }
 
        if (mpcFactor > 0.9)
        {
            isDisturbance = false;
        }
 
        // run DMP one after another
        Eigen::Matrix4f targetPose = Eigen::Matrix4f::Identity();
 
        Eigen::VectorXf dmpVels;
        dmpVels.resize(6);
        dmpVels.setZero();
        for (size_t i = 0; i < static_cast<size_t>(cfg->dmpNum); ++i)
        {
            double timeDuration = timeDurations[i];
            std::string dmpType = dmpTypes[i];
 
            //double amplitude = 1.0;
            if (dmpType == "Periodic")
            {
                if (canVals[i] <= 1e-8)
                {
                    canVals[i] = timeDuration;
                }
                //amplitude = amplitudes[i];
            }
 
            if (canVals[i] > 1e-8)
            {
                DMP::UMITSMPPtr dmpPtr = dmpPtrList[i];
                double dmpDeltaT = deltaT / timeDuration;
                double tau = dmpPtr->getTemporalFactor();
                canVals[i] -= 1 / tau * deltaT * 1 / (1 + phaseStop);
 
 
                currentStates[i] = dmpPtr->calculateDirectlyVelocity(
                    currentStates[i], canVals[i] / timeDurations[i], dmpDeltaT, targetSubStates[i]);
 
 
                for (size_t j = 0; j < 3; ++j)
                {
                    dmpVels(j) += currentStates[i][j].vel / timeDurations[i];
                }
 
                Eigen::Quaterniond quatVel0;
                quatVel0.w() = currentStates[i][3].vel;
                quatVel0.x() = currentStates[i][4].vel;
                quatVel0.y() = currentStates[i][5].vel;
                quatVel0.z() = currentStates[i][6].vel;
                Eigen::Quaterniond dmpQ;
                dmpQ.w() = currentStates[i][3].pos;
                dmpQ.x() = currentStates[i][4].pos;
                dmpQ.y() = currentStates[i][5].pos;
                dmpQ.z() = currentStates[i][6].pos;
                //Eigen::Quaterniond angularVel0 = 2.0 * quatVel0 * dmpQ.inverse();
                const Eigen::Quaterniond aVtmp0 = quatVel0 * dmpQ.inverse();
                const Eigen::Quaterniond angularVel0{
                    2 * aVtmp0.w(), 2 * aVtmp0.x(), 2 * aVtmp0.y(), 2 * aVtmp0.z()};
 
 
                Eigen::Vector3f angularVelVec;
                angularVelVec << angularVel0.x() / timeDurations[i],
                    angularVel0.y() / timeDurations[i], angularVel0.z() / timeDurations[i];
 
                angularVelVec = targetPose.block<3, 3>(0, 0) * angularVelVec;
 
                ARMARX_INFO << "i: " << i << " angularVelVec: " << angularVelVec;
                dmpVels(3) += angularVelVec(0);
                dmpVels(4) += angularVelVec(1);
                dmpVels(5) += angularVelVec(2);
 
                finished = false;
            }
 
 
            Eigen::Matrix4f targetSubMat =
                VirtualRobot::MathTools::quat2eigen4f(targetSubStates[i][4],
                                                      targetSubStates[i][5],
                                                      targetSubStates[i][6],
                                                      targetSubStates[i][3]);
            targetSubMat(0, 3) = targetSubStates[i][0];
            targetSubMat(1, 3) = targetSubStates[i][1];
            targetSubMat(2, 3) = targetSubStates[i][2];
 
            targetPose = targetPose * targetSubMat;
        }
 
        //        ARMARX_INFO << "targetPose: " << targetPose;
        for (size_t i = 0; i < 3; i++)
        {
            double vel0 = dmpVels(i);
            double vel1 = phaseKpPos * (targetState[i] - currentPose(i, 3));
            targetVels(i) = mpcFactor * vel0 + (1 - mpcFactor) * vel1;
        }
 
 
        Eigen::Quaterniond diffQ = targetQ * currentQ.inverse();
        const Eigen::Quaterniond quatVel1{phaseKpOri * diffQ.w(),
                                          phaseKpOri * diffQ.x(),
                                          phaseKpOri * diffQ.y(),
                                          phaseKpOri * diffQ.z()};
        //Eigen::Quaterniond angularVel1 = 2.0 * quatVel1 * currentQ.inverse();
        const Eigen::Quaterniond aVtmp1 = quatVel1 * currentQ.inverse();
        const Eigen::Quaterniond angularVel1{
            2 * aVtmp1.w(), 2 * aVtmp1.x(), 2 * aVtmp1.y(), 2 * aVtmp1.z()};
        targetVels(3) = mpcFactor * dmpVels(3) + (1 - mpcFactor) * angularVel1.x();
        targetVels(4) = mpcFactor * dmpVels(4) + (1 - mpcFactor) * angularVel1.y();
        targetVels(5) = mpcFactor * dmpVels(5) + (1 - mpcFactor) * angularVel1.z();
 
 
        VirtualRobot::MathTools::Quaternion tQuat =
            VirtualRobot::MathTools::eigen4f2quat(targetPose);
        targetState[0] = targetPose(0, 3);
        targetState[1] = targetPose(1, 3);
        targetState[2] = targetPose(2, 3);
        targetState[3] = tQuat.w;
        targetState[4] = tQuat.x;
        targetState[5] = tQuat.y;
        targetState[6] = tQuat.z;
 
 
        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().realTCP["real_x"] = currentPosition[0];
        debugOutputData.getWriteBuffer().realTCP["real_y"] = currentPosition[1];
        debugOutputData.getWriteBuffer().realTCP["real_z"] = currentPosition[2];
        debugOutputData.getWriteBuffer().realTCP["real_qw"] = cQuat.w;
        debugOutputData.getWriteBuffer().realTCP["real_qx"] = cQuat.x;
        debugOutputData.getWriteBuffer().realTCP["real_qy"] = cQuat.y;
        debugOutputData.getWriteBuffer().realTCP["real_qz"] = cQuat.z;
 
        debugOutputData.getWriteBuffer().mpcFactor = mpcFactor;
        debugOutputData.getWriteBuffer().error = error;
        debugOutputData.getWriteBuffer().phaseStop = phaseStop;
        debugOutputData.getWriteBuffer().posError = posError;
        debugOutputData.getWriteBuffer().oriError = oriError;
        debugOutputData.getWriteBuffer().deltaT = deltaT;
 
        debugOutputData.getWriteBuffer().canVal0 = canVals[0];
 
 
        debugOutputData.commitWrite();
 
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().targetTSVel = targetVels;
        writeControlStruct();
    }
 
    void
    NJointCCDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                 const IceUtil::Time& timeSinceLastIteration)
    {
        double deltaT = timeSinceLastIteration.toSecondsDouble();
        controllerSensorData.getWriteBuffer().currentPose = tcp->getPoseInRootFrame();
        controllerSensorData.getWriteBuffer().deltaT = deltaT;
        controllerSensorData.getWriteBuffer().currentTime += deltaT;
        controllerSensorData.commitWrite();
        // cartesian vel controller
 
        Eigen::MatrixXf jacobi =
            ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
 
 
        Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(
            jacobi, ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
 
        Eigen::VectorXf jnv = jtpinv * rtGetControlStruct().targetTSVel;
 
        for (size_t i = 0; i < targets.size(); ++i)
        {
            targets.at(i)->velocity = jnv(i);
        }
    }
 
    void
    NJointCCDMPController::learnDMPFromFiles(int dmpId,
                                             const Ice::StringSeq& fileNames,
                                             const Ice::Current&)
    {
        DMP::Vec<DMP::SampledTrajectoryV2> trajs;
 
        DMP::DVec ratios;
        DMP::DVec goals;
        DMP::Vec<DMP::DMPState> starts;
 
        for (size_t i = 0; i < fileNames.size(); ++i)
        {
            DMP::SampledTrajectoryV2 traj;
            traj.readFromCSVFile(fileNames.at(i));
            traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
            trajs.push_back(traj);
 
            if (i == 0)
            {
                goals.resize(traj.dim());
                starts.resize(traj.dim());
                for (size_t j = 0; j < goals.size(); ++j)
                {
                    goals[j] = traj.rbegin()->getPosition(j);
                    starts[j].pos = traj.begin()->getPosition(j);
                    starts[j].vel = traj.begin()->getDeriv(j, 1) * timeDurations[dmpId];
                }
            }
 
            if (i == 0)
            {
                ratios.push_back(1.0);
            }
            else
            {
                ratios.push_back(0.0);
            }
        }
 
        dmpPtrList[dmpId]->learnFromTrajectories(trajs);
        dmpPtrList[dmpId]->styleParas = dmpPtrList[dmpId]->getStyleParasWithRatio(ratios);
 
 
        DMP::Vec<DMP::DMPState> currentState;
        if (dmpId == 0)
        {
            for (size_t i = 0; i < 3; i++)
            {
                DMP::DMPState currentPos;
                currentPos.pos = tcpPosition(i);
                currentPos.vel = 0;
                currentState.push_back(currentPos);
            }
 
            DMP::DMPState currentPos;
            currentPos.pos = tcpOrientation.w();
            currentPos.vel = 0;
            currentState.push_back(currentPos);
 
            currentPos.pos = tcpOrientation.x();
            currentPos.vel = 0;
            currentState.push_back(currentPos);
 
            currentPos.pos = tcpOrientation.y();
            currentPos.vel = 0;
            currentState.push_back(currentPos);
 
            currentPos.pos = tcpOrientation.z();
            currentPos.vel = 0;
            currentState.push_back(currentPos);
        }
        else
        {
            currentState = starts;
        }
        for (size_t i = 0; i < 3; i++)
        {
            targetState[i] = tcpPosition(i);
        }
 
        targetState[3] = tcpOrientation.w();
        targetState[4] = tcpOrientation.x();
        targetState[5] = tcpOrientation.y();
        targetState[6] = tcpOrientation.z();
 
        currentStates[dmpId] = currentState;
 
        dmpPtrList[dmpId]->prepareExecution(goals, currentState, 1, tau);
        dmpPtrList[dmpId]->setTemporalFactor(tau);
 
        learnedDMP.push_back(dmpId);
        ARMARX_INFO << "Learned DMP ... ";
    }
 
    void
    NJointCCDMPController::setViaPoints(int dmpId,
                                        Ice::Double u,
                                        const Ice::DoubleSeq& viapoint,
                                        const Ice::Current&)
    {
 
        LockGuardType guard(controllerMutex);
        dmpPtrList[dmpId]->setViaPoint(u, viapoint);
    }
 
    void
    NJointCCDMPController::setGoals(int dmpId, const Ice::DoubleSeq& goals, const Ice::Current& ice)
    {
        setViaPoints(dmpId, dmpPtrList[dmpId]->getUMin(), goals, ice);
    }
 
    void
    NJointCCDMPController::setControllerTarget(
        Ice::Float avoidJointLimitsKp,
        NJointTaskSpaceDMPControllerMode::CartesianSelection mode,
        const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
        getWriterControlStruct().mode = ModeFromIce(mode);
        writeControlStruct();
    }
 
    VirtualRobot::IKSolver::CartesianSelection
    NJointCCDMPController::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
    NJointCCDMPController::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
    NJointCCDMPController::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
    NJointCCDMPController::runDMP(const Ice::Current&)
    {
 
        const auto dmpNum = static_cast<std::size_t>(cfg->dmpNum);
        finished = false;
        if (dmpNum != dmpTypes.size() || dmpNum != dmpPtrList.size() ||
            dmpNum != learnedDMP.size() || dmpNum != canVals.size() ||
            dmpNum != currentStates.size() || dmpNum != targetSubStates.size())
        {
            ARMARX_ERROR << "Error: cannot run CCDMP controller. The reason is that some "
                            "parameters have different sizes";
            return;
        }
        ARMARX_INFO << "run DMP";
        controllerTask->start();
    }
 
    void
    NJointCCDMPController::setTemporalFactor(int dmpId, double tau, const Ice::Current&)
    {
        LockGuardType guard(controllerMutex);
        dmpPtrList[dmpId]->setTemporalFactor(tau);
    }
 
    void
    NJointCCDMPController::rtPreActivateController()
    {
    }
 
    void
    NJointCCDMPController::rtPostDeactivateController()
    {
    }
 
    void
    NJointCCDMPController::onPublish(const SensorAndControl&,
                                     const DebugDrawerInterfacePrx&,
                                     const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
        auto values = debugOutputData.getUpToDateReadBuffer().latestTargetVelocities;
        for (auto& pair : values)
        {
            datafields[pair.first] = new Variant(pair.second);
        }
 
        auto dmpTargets = debugOutputData.getUpToDateReadBuffer().dmpTargets;
        for (auto& pair : dmpTargets)
        {
            datafields[pair.first] = new Variant(pair.second);
        }
 
        auto realTCP = debugOutputData.getUpToDateReadBuffer().realTCP;
        for (auto& pair : realTCP)
        {
            datafields[pair.first] = new Variant(pair.second);
        }
 
 
        datafields["mpcFactor"] = new Variant(debugOutputData.getUpToDateReadBuffer().mpcFactor);
        datafields["poseError"] = new Variant(debugOutputData.getUpToDateReadBuffer().error);
        datafields["phaseStop"] = new Variant(debugOutputData.getUpToDateReadBuffer().phaseStop);
        datafields["posError"] = new Variant(debugOutputData.getUpToDateReadBuffer().posError);
        datafields["oriError"] = new Variant(debugOutputData.getUpToDateReadBuffer().oriError);
        datafields["deltaT"] = new Variant(debugOutputData.getUpToDateReadBuffer().deltaT);
        datafields["canVal0"] = new Variant(debugOutputData.getUpToDateReadBuffer().canVal0);
 
        debugObs->setDebugChannel("DMPController", datafields);
    }
 
    void
    NJointCCDMPController::onInitNJointController()
    {
        ARMARX_INFO << "init ...";
        controllerTask = new PeriodicTask<NJointCCDMPController>(
            this, &NJointCCDMPController::controllerRun, 0.3);
    }
 
    void
    NJointCCDMPController::onDisconnectNJointController()
    {
        controllerTask->stop();
        ARMARX_INFO << "stopped ...";
    }
 
 
} // namespace armarx::control::deprecated_njoint_mp_controller::task_space