DeprecatedNJointTSDMPController.cpp

Go to the documentation of this file.

#include "DeprecatedNJointTSDMPController.h"
 
#include <algorithm>
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.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
{
 
    struct RTScopeTimer
    {
        RTScopeTimer(const std::string& name) : start(armarx::rtNow()), name(name)
        {
        }
 
        ~RTScopeTimer()
        {
            //float us = (armarx::rtNow() - start).toMicroSecondsDouble();
            //ARMARX_IMPORTANT << name << " took " << us << " micro seconds";
        }
 
        IceUtil::Time start;
        std::string name;
    };
 
    NJointControllerRegistration<DeprecatedNJointTSDMPController>
        registrationControllerDeprecatedNJointTSDMPController("DeprecatedNJointTSDMPController");
 
    DeprecatedNJointTSDMPController::DeprecatedNJointTSDMPController(
        const RobotUnitPtr&,
        const armarx::NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        RTScopeTimer timer("DeprecatedNJointTSDMPController_constructor");
        RT_TIMING_START(DeprecatedNJointTSDMPController_constructor)
        useSynchronizedRtRobot();
        cfg = DeprecatedNJointTaskSpaceDMPControllerConfigPtr::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);
        };
        forceSensor = useSensorValue(cfg->forceSensorName)->asA<SensorValueForceTorque>();
 
        ARMARX_TRACE;
        forceOffset.setZero();
        filteredForce.setZero();
        filteredForceInRoot.setZero();
        forceThreshold.reinitAllBuffers(cfg->forceThreshold);
        useForceStop = false;
 
        if (not cfg->tcpName.empty())
        {
            ARMARX_INFO << "Custom TCP " << cfg->tcpName << " provided.";
        }
 
        tcp = cfg->tcpName.empty() ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName);
 
        ARMARX_INFO << "Using TCP: " << tcp->getName();
 
        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);
        DeprecatedNJointTSDMPControllerControlData 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);
        RT_TIMING_END(DeprecatedNJointTSDMPController_constructor)
    }
 
    std::string
    DeprecatedNJointTSDMPController::getClassName(const Ice::Current&) const
    {
        return "DeprecatedNJointTSDMPController";
    }
 
    void
    DeprecatedNJointTSDMPController::controllerRun()
    {
        RTScopeTimer timer("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
    DeprecatedNJointTSDMPController::calcIK(const Eigen::VectorXf& cartesianVel,
                                            const Eigen::VectorXf& nullspaceVel,
                                            VirtualRobot::IKSolver::CartesianSelection mode)
    {
        RTScopeTimer timer("calcIK");
        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
    DeprecatedNJointTSDMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                           const IceUtil::Time& timeSinceLastIteration)
    {
        // RT_TIMING_START(DeprecatedNJointTSDMPController_rtRun)
        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);
            if (not started)
            {
                forceOffset =
                    (1 - cfg->forceFilter) * forceOffset + cfg->forceFilter * forceSensor->force;
                timeForCalibration = timeForCalibration + timeSinceLastIteration.toSecondsDouble();
            }
            if (started and 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()->getRobotNode(cfg->forceFrameName)->getPoseInRootFrame();
                filteredForceInRoot = forceFrameInRoot.block<3, 3>(0, 0) * filteredForce;
 
                for (size_t i = 0; i < 3; ++i)
                {
                    if (fabs(filteredForceInRoot[i]) > forceThreshold.getUpToDateReadBuffer()[i])
                    {
                        started = false;
                        break;
                    }
                }
            }
        }
 
        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);
        }
 
 
        // If the controller is stopped, set the velocity to 0
        // and skip the rest of the loop. This ensures that no velocity from the controller
        // is sent to the targets when the controller is stopped.
        if (stopped.load())
        {
            // ARMARX_INFO << deactivateSpam(1.) << "Controller is stopped. Setting velocity to 0";
            std::for_each(
                targets.begin(), targets.end(), [](auto* target) { target->velocity = 0; });
        }
        else
        {
            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();
        // RT_TIMING_CEND(DeprecatedNJointTSDMPController_rtRun, 0.1)
    }
 
    void
    DeprecatedNJointTSDMPController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
                                                       const Ice::Current&)
    {
        RTScopeTimer timer("learnDMPFromFiles");
        ARMARX_INFO << "Learning DMP ... ";
 
        LockGuardType guard{controllerMutex};
        dmpCtrl->learnDMPFromFiles(fileNames);
    }
 
    void
    DeprecatedNJointTSDMPController::setSpeed(Ice::Double times, const Ice::Current&)
    {
        RTScopeTimer timer("setSpeed");
        LockGuardType guard{controllerMutex};
        dmpCtrl->setSpeed(times);
    }
 
    void
    DeprecatedNJointTSDMPController::setViaPoints(Ice::Double u,
                                                  const Ice::DoubleSeq& viapoint,
                                                  const Ice::Current&)
    {
        RTScopeTimer timer("setViaPoints");
        LockGuardType guard{controllerMutex};
        dmpCtrl->setViaPose(u, viapoint);
    }
 
    void
    DeprecatedNJointTSDMPController::setTorqueKp(const StringFloatDictionary& torqueKp,
                                                 const Ice::Current&)
    {
        RTScopeTimer timer("setTorqueKp");
        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
    DeprecatedNJointTSDMPController::setNullspaceJointVelocities(
        const StringFloatDictionary& nullspaceJointVelocities,
        const Ice::Current&)
    {
        RTScopeTimer timer("setNullspaceJointVelocities");
        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
    DeprecatedNJointTSDMPController::setControllerTarget(
        Ice::Float avoidJointLimitsKp,
        NJointTaskSpaceDMPControllerMode::CartesianSelection mode,
        const Ice::Current&)
    {
        RTScopeTimer timer("setControllerTarget");
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().avoidJointLimitsKp = avoidJointLimitsKp;
        getWriterControlStruct().mode = ModeFromIce(mode);
        writeControlStruct();
    }
 
    void
    DeprecatedNJointTSDMPController::removeAllViaPoints(const Ice::Current&)
    {
        RTScopeTimer timer("removeAllViaPoints");
        LockGuardType guard{controllerMutex};
        ARMARX_INFO << "setting via points ";
        dmpCtrl->removeAllViaPoints();
    }
 
    void
    DeprecatedNJointTSDMPController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
    {
        RTScopeTimer timer("setGoals");
        LockGuardType guard{controllerMutex};
        dmpCtrl->setGoalPoseVec(goals);
    }
 
    void
    DeprecatedNJointTSDMPController::pauseDMP(const Ice::Current&)
    {
        RTScopeTimer timer("pauseDMP");
        dmpCtrl->pauseController();
    }
 
    void
    DeprecatedNJointTSDMPController::resumeDMP(const Ice::Current&)
    {
        RTScopeTimer timer("resumeDMP");
        dmpCtrl->resumeController();
    }
 
    void
    DeprecatedNJointTSDMPController::resetDMP(const Ice::Current&)
    {
        RTScopeTimer timer("resetDMP");
        if (started)
        {
            ARMARX_INFO << "Cannot reset running DMP";
        }
        firstRun = true;
    }
 
    void
    DeprecatedNJointTSDMPController::stopDMP(const Ice::Current& c)
    {
        RTScopeTimer timer("stopDMP");
        started = false;
        stopped = true;
 
        // If the NJoint controller is stopped, the underlying controller should be stopped as well.
        // Note: resumeDMP() does not work if the controller is stopped.
        pauseDMP(c);
    }
 
    std::string
    DeprecatedNJointTSDMPController::getDMPAsString(const Ice::Current&)
    {
        RTScopeTimer timer("getDMPAsString");
        return dmpCtrl->saveDMPToString();
    }
 
    std::vector<double>
    DeprecatedNJointTSDMPController::createDMPFromString(const std::string& dmpString,
                                                         const Ice::Current&)
    {
        RTScopeTimer timer("createDMPFromString");
        dmpCtrl->loadDMPFromString(dmpString);
        return dmpCtrl->dmpPtr->defaultGoal;
    }
 
    VirtualRobot::IKSolver::CartesianSelection
    DeprecatedNJointTSDMPController::ModeFromIce(
        const NJointTaskSpaceDMPControllerMode::CartesianSelection mode)
    {
        RTScopeTimer timer("ModeFromIce");
        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
    DeprecatedNJointTSDMPController::runDMP(const Ice::DoubleSeq& goals,
                                            double tau,
                                            const Ice::Current&)
    {
        RTScopeTimer timer("runDMP");
        ARMARX_INFO << "------dmp controller: " << VAROUT(goals);
        firstRun = true;
 
        stopped = false;
        timeForCalibration = 0;
        while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
        {
            if (stopped.load())
            {
                ARMARX_WARNING << "StopDMP has been prematurely called; aborting runDMP";
                return;
            }
            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);
        dmpCtrl->resumeController();
        
        finished = false;
 
        ARMARX_INFO << "run DMP";
        started = true;
    }
 
    void
    DeprecatedNJointTSDMPController::runDMPWithTime(const Ice::DoubleSeq& goals,
                                                    Ice::Double timeDuration,
                                                    const Ice::Current&)
    {
        RTScopeTimer timer("runDMPWithTime");
        firstRun = true;
        stopped = false;
 
        while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
        {
            if (stopped.load())
            {
                ARMARX_WARNING << "StopDMP has been prematurely called; aborting runDMP";
                return;
            }
            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);
        dmpCtrl->resumeController();
 
        finished = false;
        started = true;
    }
 
    void
    DeprecatedNJointTSDMPController::rtPreActivateController()
    {
    }
 
    void
    DeprecatedNJointTSDMPController::rtPostDeactivateController()
    {
        ARMARX_INFO << "controller has been deactivated";
    }
 
    void
    DeprecatedNJointTSDMPController::onPublish(const SensorAndControl&,
                                               const DebugDrawerInterfacePrx&,
                                               const DebugObserverInterfacePrx& debugObs)
    {
        RTScopeTimer timer("onPublish");
        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);
        ARMARX_VERBOSE << deactivateSpam(1)
                       << "Error: " << debugOutputData.getUpToDateReadBuffer().error
                       << "\nPosition Error: " << debugOutputData.getUpToDateReadBuffer().posError
                       << "\nOrientation Error: "
                       << debugOutputData.getUpToDateReadBuffer().oriError;
 
        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
    DeprecatedNJointTSDMPController::onInitNJointController()
    {
        RTScopeTimer timer("onInitNJointController");
        ARMARX_INFO << "init ...";
        started = false;
        runTask("DeprecatedNJointTSDMPController",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            controllerRun();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    DeprecatedNJointTSDMPController::onDisconnectNJointController()
    {
        ARMARX_INFO << "stopped ...";
    }
 
    void
    DeprecatedNJointTSDMPController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
    {
        RTScopeTimer timer("setMPWeights");
        dmpCtrl->setWeights(weights);
    }
 
    DoubleSeqSeq
    DeprecatedNJointTSDMPController::getMPWeights(const Ice::Current&)
    {
        RTScopeTimer timer("getMPWeights");
        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
    DeprecatedNJointTSDMPController::setLinearVelocityKd(Ice::Float kd, const Ice::Current&)
    {
        DpF = kd;
    }
 
    void
    DeprecatedNJointTSDMPController::setLinearVelocityKp(Ice::Float kp, const Ice::Current&)
    {
        KpF = kp;
    }
 
    void
    DeprecatedNJointTSDMPController::setAngularVelocityKd(Ice::Float kd, const Ice::Current&)
    {
        DoF = kd;
    }
 
    void
    DeprecatedNJointTSDMPController::setAngularVelocityKp(Ice::Float kp, const Ice::Current&)
    {
        KoF = kp;
    }
 
} // namespace armarx::control::deprecated_njoint_mp_controller::task_space