NJointBimanualForceMPController.cpp

Go to the documentation of this file.

#include "NJointBimanualForceMPController.h"
 
#include <random>
 
// Simox
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
// DMP
#include <dmp/representation/dmp/umitsmp.h>
 
// armarx
#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 <RobotAPI/libraries/core/CartesianVelocityController.h>
#include <RobotAPI/libraries/core/PIDController.h>
 
// control
 
 
namespace armarx::control::deprecated_njoint_mp_controller::bimanual
{
    NJointControllerRegistration<NJointBimanualForceMPController>
        registrationControllerNJointBimanualForceMPController("NJointBimanualForceMPController");
 
    NJointBimanualForceMPController::NJointBimanualForceMPController(
        const RobotUnitPtr& robUnit,
        const armarx::NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        useSynchronizedRtRobot();
        cfg = NJointBimanualForceMPControllerConfigPtr::dynamicCast(config);
 
        VirtualRobot::RobotNodeSetPtr lrns = rtGetRobot()->getRobotNodeSet("LeftArm");
        for (size_t i = 0; i < lrns->getSize(); ++i)
        {
            std::string jointName = lrns->getNode(i)->getName();
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No velocity sensor available for " << jointName;
            }
            if (!positionSensor)
            {
                ARMARX_WARNING << "No position sensor available for " << jointName;
            }
 
            leftVelocitySensors.push_back(velocitySensor);
            leftPositionSensors.push_back(positionSensor);
        };
 
        VirtualRobot::RobotNodeSetPtr rrns = rtGetRobot()->getRobotNodeSet("RightArm");
        for (size_t i = 0; i < rrns->getSize(); ++i)
        {
            std::string jointName = rrns->getNode(i)->getName();
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No velocity sensor available for " << jointName;
            }
            if (!positionSensor)
            {
                ARMARX_WARNING << "No position sensor available for " << jointName;
            }
 
            rightVelocitySensors.push_back(velocitySensor);
            rightPositionSensors.push_back(positionSensor);
        };
        const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
        leftForceTorque = svlf->asA<SensorValueForceTorque>();
        const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
        rightForceTorque = svrf->asA<SensorValueForceTorque>();
 
        leftIK.reset(new VirtualRobot::DifferentialIK(
            lrns, lrns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
        rightIK.reset(new VirtualRobot::DifferentialIK(
            rrns, rrns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
 
        leftTCP = lrns->getTCP();
        rightTCP = rrns->getTCP();
 
        leftLearned = false;
        rightLearned = false;
 
        // set tcp controller
        leftTCPController.reset(new CartesianVelocityController(lrns, leftTCP));
        rightTCPController.reset(new CartesianVelocityController(rrns, rightTCP));
 
 
        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;
        leftDMPController.reset(
            new tsvmp::TaskSpaceDMPController("Left", taskSpaceDMPConfig, false));
        rightDMPController.reset(
            new tsvmp::TaskSpaceDMPController("Right", taskSpaceDMPConfig, false));
 
 
        // initialize tcp position and orientation
        NJointBimanualForceMPControllerSensorData initSensorData;
        initSensorData.leftPoseInRootFrame = leftTCP->getPoseInRootFrame();
        initSensorData.leftTwistInRootFrame.setZero();
        initSensorData.rightPoseInRootFrame = rightTCP->getPoseInRootFrame();
        initSensorData.rightTwistInRootFrame.setZero();
        initSensorData.leftForceInRootFrame.setZero();
        initSensorData.rightForceInRootFrame.setZero();
        controllerSensorData.reinitAllBuffers(initSensorData);
 
        NJointBimanualForceMPControllerControlData initData;
        initData.leftTargetPose = leftTCP->getPoseInRootFrame();
        initData.rightTargetPose = rightTCP->getPoseInRootFrame();
        reinitTripleBuffer(initData);
 
 
        Kp_LinearVel = cfg->Kp_LinearVel;
        Kp_AngularVel = cfg->Kp_AngularVel;
        Kd_LinearVel = cfg->Kd_LinearVel;
        Kd_AngularVel = cfg->Kd_AngularVel;
 
        forceIterm = 0;
        I_decay = 0.9;
        xvel = 0;
 
        leftFilteredValue.setZero();
        rightFilteredValue.setZero();
 
        for (size_t i = 0; i < 3; ++i)
        {
            leftForceOffset(i) = cfg->leftForceOffset.at(i);
        }
        for (size_t i = 0; i < 3; ++i)
        {
            rightForceOffset(i) = cfg->rightForceOffset.at(i);
        }
 
 
        targetSupportForce = cfg->targetSupportForce;
 
        NJointBimanualForceMPControllerInterfaceData initInterfaceData;
        initInterfaceData.leftTcpPoseInRootFrame = leftTCP->getPoseInRootFrame();
        initInterfaceData.rightTcpPoseInRootFrame = rightTCP->getPoseInRootFrame();
        interfaceData.reinitAllBuffers(initInterfaceData);
    }
 
    std::string
    NJointBimanualForceMPController::getClassName(const Ice::Current&) const
    {
        return "NJointBimanualForceMPController";
    }
 
    void
    NJointBimanualForceMPController::controllerRun()
    {
        if (!controllerSensorData.updateReadBuffer() || !leftDMPController || !rightDMPController)
        {
            return;
        }
        double deltaT = controllerSensorData.getReadBuffer().deltaT;
        Eigen::Matrix4f leftPose = controllerSensorData.getReadBuffer().leftPoseInRootFrame;
        Eigen::Matrix4f rightPose = controllerSensorData.getReadBuffer().rightPoseInRootFrame;
        Eigen::Vector6f leftTwist = controllerSensorData.getReadBuffer().leftTwistInRootFrame;
        Eigen::Vector6f rightTwist = controllerSensorData.getReadBuffer().rightTwistInRootFrame;
        Eigen::Vector3f leftForce = controllerSensorData.getReadBuffer().leftForceInRootFrame;
        Eigen::Vector3f rightForce = controllerSensorData.getReadBuffer().rightForceInRootFrame;
 
        float forceOnHands = (leftForce + rightForce)(2);
 
        xvel =
            cfg->forceP * (targetSupportForce + forceOnHands) + I_decay * cfg->forceI * forceIterm;
 
        forceIterm += targetSupportForce - forceOnHands;
 
        canVal = canVal + forceSign * xvel * deltaT;
        //        canVal = cfg->timeDuration + forceSign * xvel;
 
 
        if (canVal > cfg->timeDuration)
        {
            canVal = cfg->timeDuration;
        }
 
 
        leftDMPController->canVal = canVal;
        rightDMPController->canVal = canVal;
 
        leftDMPController->flow(deltaT, leftPose, leftTwist);
        rightDMPController->flow(deltaT, rightPose, rightTwist);
 
        if (canVal < 1e-8)
        {
            finished = true;
        }
 
 
        std::vector<double> leftTargetState = leftDMPController->getTargetPose();
        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_x"] = leftTargetState[0];
        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_y"] = leftTargetState[1];
        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_z"] = leftTargetState[2];
        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qw"] = leftTargetState[3];
        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qx"] = leftTargetState[4];
        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qy"] = leftTargetState[5];
        debugOutputData.getWriteBuffer().dmpTargets["leftTarget_qz"] = leftTargetState[6];
        std::vector<double> rightTargetState = rightDMPController->getTargetPose();
        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_x"] = rightTargetState[0];
        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_y"] = rightTargetState[1];
        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_z"] = rightTargetState[2];
        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qw"] = rightTargetState[3];
        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qx"] = rightTargetState[4];
        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qy"] = rightTargetState[5];
        debugOutputData.getWriteBuffer().dmpTargets["rightTarget_qz"] = rightTargetState[6];
 
 
        {
            debugOutputData.getWriteBuffer().currentPose["leftPose_x"] = leftPose(0, 3);
            debugOutputData.getWriteBuffer().currentPose["leftPose_y"] = leftPose(1, 3);
            debugOutputData.getWriteBuffer().currentPose["leftPose_z"] = leftPose(2, 3);
            VirtualRobot::MathTools::Quaternion leftQuat =
                VirtualRobot::MathTools::eigen4f2quat(leftPose);
            debugOutputData.getWriteBuffer().currentPose["leftPose_qw"] = leftQuat.w;
            debugOutputData.getWriteBuffer().currentPose["leftPose_qx"] = leftQuat.x;
            debugOutputData.getWriteBuffer().currentPose["leftPose_qy"] = leftQuat.y;
            debugOutputData.getWriteBuffer().currentPose["leftPose_qz"] = leftQuat.z;
        }
        {
            debugOutputData.getWriteBuffer().currentPose["rightPose_x"] = rightPose(0, 3);
            debugOutputData.getWriteBuffer().currentPose["rightPose_y"] = rightPose(1, 3);
            debugOutputData.getWriteBuffer().currentPose["rightPose_z"] = rightPose(2, 3);
            VirtualRobot::MathTools::Quaternion rightQuat =
                VirtualRobot::MathTools::eigen4f2quat(rightPose);
            debugOutputData.getWriteBuffer().currentPose["rightPose_qw"] = rightQuat.w;
            debugOutputData.getWriteBuffer().currentPose["rightPose_qx"] = rightQuat.x;
            debugOutputData.getWriteBuffer().currentPose["rightPose_qy"] = rightQuat.y;
            debugOutputData.getWriteBuffer().currentPose["rightPose_qz"] = rightQuat.z;
        }
 
        debugOutputData.getWriteBuffer().canVal = canVal;
        debugOutputData.getWriteBuffer().forceOnHands = forceOnHands;
        debugOutputData.commitWrite();
 
 
        getWriterControlStruct().leftTargetPose = leftDMPController->getTargetPoseMat();
        getWriterControlStruct().rightTargetPose = rightDMPController->getTargetPoseMat();
        writeControlStruct();
    }
 
    void
    NJointBimanualForceMPController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                           const IceUtil::Time& timeSinceLastIteration)
    {
        Eigen::Matrix4f leftPose = leftTCP->getPoseInRootFrame();
        Eigen::Matrix4f rightPose = rightTCP->getPoseInRootFrame();
        Eigen::MatrixXf leftJacobi =
            leftIK->getJacobianMatrix(leftTCP, VirtualRobot::IKSolver::CartesianSelection::All);
        Eigen::MatrixXf rightJacobi =
            leftIK->getJacobianMatrix(leftTCP, VirtualRobot::IKSolver::CartesianSelection::All);
 
        Eigen::VectorXf qvel;
        qvel.resize(leftVelocitySensors.size());
        for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
        {
            qvel(i) = leftVelocitySensors[i]->velocity;
        }
        Eigen::Vector6f leftTwist = leftJacobi * qvel;
        for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
        {
            qvel(i) = rightVelocitySensors[i]->velocity;
        }
        Eigen::Vector6f rightTwist = rightJacobi * qvel;
 
        leftFilteredValue = (1 - cfg->filterCoeff) * (leftForceTorque->force - leftForceOffset) +
                            cfg->filterCoeff * leftFilteredValue;
        rightFilteredValue = (1 - cfg->filterCoeff) * (rightForceTorque->force - rightForceOffset) +
                             cfg->filterCoeff * rightFilteredValue;
 
        Eigen::Matrix4f leftSensorFrame =
            rtGetRobot()->getRobotNode("ArmL8_Wri2")->getPoseInRootFrame();
        Eigen::Matrix4f rightSensorFrame =
            rtGetRobot()->getRobotNode("ArmR8_Wri2")->getPoseInRootFrame();
 
        Eigen::Vector3f leftForceInRootFrame = leftPose.block<3, 3>(0, 0).transpose() *
                                               leftSensorFrame.block<3, 3>(0, 0) *
                                               leftFilteredValue;
        Eigen::Vector3f rightForceInRootFrame = rightPose.block<3, 3>(0, 0).transpose() *
                                                rightSensorFrame.block<3, 3>(0, 0) *
                                                rightFilteredValue;
 
        controllerSensorData.getWriteBuffer().leftPoseInRootFrame = leftPose;
        controllerSensorData.getWriteBuffer().rightPoseInRootFrame = rightPose;
        controllerSensorData.getWriteBuffer().leftTwistInRootFrame = leftTwist;
        controllerSensorData.getWriteBuffer().rightTwistInRootFrame = rightTwist;
        controllerSensorData.getWriteBuffer().leftForceInRootFrame = leftForceInRootFrame;
        controllerSensorData.getWriteBuffer().rightForceInRootFrame = rightForceInRootFrame;
        controllerSensorData.getWriteBuffer().deltaT = timeSinceLastIteration.toSecondsDouble();
        controllerSensorData.commitWrite();
 
        interfaceData.getWriteBuffer().leftTcpPoseInRootFrame = leftPose;
        interfaceData.getWriteBuffer().rightTcpPoseInRootFrame = rightPose;
        interfaceData.commitWrite();
 
 
        Eigen::Matrix4f targetPose = rtGetControlStruct().leftTargetPose;
        Eigen::Matrix4f currentPose = leftPose;
        Eigen::Vector6f leftVel;
 
        {
            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) =
                Kp_LinearVel * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) +
                Kd_LinearVel * (-leftTwist.block<3, 1>(0, 0));
            rtTargetVel.block<3, 1>(3, 0) =
                Kp_AngularVel * errorRPY + Kd_AngularVel * (-leftTwist.block<3, 1>(3, 0));
 
            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;
            }
 
            for (size_t i = 0; i < 6; i++)
            {
                leftVel(i) = rtTargetVel(i);
            }
        }
 
        // cartesian vel controller
 
 
        targetPose = rtGetControlStruct().rightTargetPose;
        currentPose = rightPose;
        Eigen::Vector6f rightVel;
 
        {
            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) =
                Kp_LinearVel * (targetPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)) +
                Kd_LinearVel * (-rightTwist.block<3, 1>(0, 0));
            rtTargetVel.block<3, 1>(3, 0) =
                Kp_AngularVel * errorRPY + Kd_AngularVel * (-rightTwist.block<3, 1>(3, 0));
 
            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;
            }
 
            for (size_t i = 0; i < 6; i++)
            {
                rightVel(i) = rtTargetVel(i);
            }
        }
 
 
        Eigen::VectorXf leftJTV =
            leftTCPController->calculate(leftVel,
                                         cfg->KpJointLimitAvoidanceScale,
                                         VirtualRobot::IKSolver::CartesianSelection::All);
        for (size_t i = 0; i < leftTargets.size(); ++i)
        {
            leftTargets.at(i)->velocity = leftJTV(i);
            if (!leftTargets.at(i)->isValid())
            {
                ARMARX_IMPORTANT
                    << deactivateSpam(1, std::to_string(i))
                    << "Velocity controller target is invalid - setting to zero! set value: "
                    << leftTargets.at(i)->velocity;
                leftTargets.at(i)->velocity = 0.0f;
            }
        }
 
        Eigen::VectorXf rightJTV =
            rightTCPController->calculate(rightVel,
                                          cfg->KpJointLimitAvoidanceScale,
                                          VirtualRobot::IKSolver::CartesianSelection::All);
        for (size_t i = 0; i < rightTargets.size(); ++i)
        {
            rightTargets.at(i)->velocity = rightJTV(i);
            if (!rightTargets.at(i)->isValid())
            {
                ARMARX_IMPORTANT
                    << deactivateSpam(1, std::to_string(i))
                    << "Velocity controller target is invalid - setting to zero! set value: "
                    << rightTargets.at(i)->velocity;
                rightTargets.at(i)->velocity = 0.0f;
            }
        }
    }
 
    void
    NJointBimanualForceMPController::learnDMPFromFiles(const std::string& whichDMP,
                                                       const Ice::StringSeq& fileNames,
                                                       const Ice::Current&)
    {
        ARMARX_IMPORTANT << "Learn DMP " << whichDMP;
        if (whichDMP == "Left")
        {
            leftDMPController->learnDMPFromFiles(fileNames);
            leftLearned = true;
            ARMARX_INFO << "Left Learned";
        }
        if (whichDMP == "Right")
        {
            rightDMPController->learnDMPFromFiles(fileNames);
            rightLearned = true;
            ARMARX_INFO << "Right Learned";
        }
    }
 
    void
    NJointBimanualForceMPController::runDMP(const Ice::DoubleSeq& leftGoals,
                                            const Ice::DoubleSeq& rightGoals,
                                            const Ice::Current&)
    {
        if (!leftLearned)
        {
            ARMARX_ERROR << "Left DMP is not learned, aborted";
            return;
        }
 
        if (!rightLearned)
        {
            ARMARX_ERROR << "Right DMP is not learned, aborted";
            return;
        }
 
        while (!interfaceData.updateReadBuffer())
        {
            usleep(100);
        }
 
        Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().leftTcpPoseInRootFrame;
        Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().rightTcpPoseInRootFrame;
 
        forceSign = leftGoals.at(2) - leftPose(2, 3) > 0 ? -1 : 1;
        //        Eigen::Matrix4f pose = tcp->getPoseInRootFrame();
        ARMARX_IMPORTANT << "leftGoals dim: " << leftGoals.size();
        ARMARX_IMPORTANT << "rightGoals dim: " << rightGoals.size();
 
        leftDMPController->prepareExecution(leftDMPController->eigen4f2vec(leftPose), leftGoals);
        rightDMPController->prepareExecution(rightDMPController->eigen4f2vec(rightPose),
                                             rightGoals);
 
        canVal = cfg->timeDuration;
        finished = false;
 
        ARMARX_INFO << "run DMP";
        controllerTask->start();
    }
 
    void
    NJointBimanualForceMPController::setViaPoints(const std::string& whichDMP,
                                                  double u,
                                                  const Ice::DoubleSeq& viaPoint,
                                                  const Ice::Current&)
    {
        if (whichDMP == "Left")
        {
            leftDMPController->setViaPose(u, viaPoint);
        }
        if (whichDMP == "Right")
        {
            rightDMPController->setViaPose(u, viaPoint);
        }
    }
 
    void
    NJointBimanualForceMPController::onPublish(const SensorAndControl&,
                                               const DebugDrawerInterfacePrx&,
                                               const DebugObserverInterfacePrx& debugObs)
    {
        std::string debugName = cfg->debugName;
        std::string datafieldName = debugName;
        StringVariantBaseMap datafields;
 
        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().canVal);
 
        datafieldName = "forceOnHands_" + debugName;
        datafields[datafieldName] =
            new Variant(debugOutputData.getUpToDateReadBuffer().forceOnHands);
        datafieldName = "DMPController_" + debugName;
 
        debugObs->setDebugChannel(datafieldName, datafields);
    }
 
    void
    NJointBimanualForceMPController::onInitNJointController()
    {
        ARMARX_INFO << "init ...";
        controllerTask = new PeriodicTask<NJointBimanualForceMPController>(
            this, &NJointBimanualForceMPController::controllerRun, 0.3);
    }
 
    void
    NJointBimanualForceMPController::onDisconnectNJointController()
    {
        ARMARX_INFO << "stopped ...";
        controllerTask->stop();
    }
 
 
} // namespace armarx::control::deprecated_njoint_mp_controller::bimanual