NJointBimanualObjLevelController.cpp

Go to the documentation of this file.

#include "NJointBimanualObjLevelController.h"
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
// Simox
#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>
 
// 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/PIDController.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
 
namespace armarx::control::deprecated_njoint_mp_controller::bimanual
{
    NJointControllerRegistration<NJointBimanualObjLevelController>
        registrationControllerNJointBimanualObjLevelController("NJointBimanualObjLevelController");
 
    NJointBimanualObjLevelController::NJointBimanualObjLevelController(
        const RobotUnitPtr& robUnit,
        const armarx::NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        ARMARX_INFO << "Initializing Bimanual Object Level Controller";
 
        useSynchronizedRtRobot();
        cfg = NJointBimanualObjLevelControllerConfigPtr::dynamicCast(config);
        //        ARMARX_CHECK_EXPRESSION(prov);
        //        RobotUnitPtr robotUnit = RobotUnitPtr::dynamicCast(prov);
        //        ARMARX_CHECK_EXPRESSION(robotUnit);
        leftRNS = rtGetRobot()->getRobotNodeSet("LeftArm");
 
        for (size_t i = 0; i < leftRNS->getSize(); ++i)
        {
            std::string jointName = leftRNS->getNode(i)->getName();
            leftJointNames.push_back(jointName);
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No velocitySensor available for " << jointName;
            }
            if (!positionSensor)
            {
                ARMARX_WARNING << "No positionSensor available for " << jointName;
            }
 
 
            leftVelocitySensors.push_back(velocitySensor);
            leftPositionSensors.push_back(positionSensor);
        };
 
        rightRNS = rtGetRobot()->getRobotNodeSet("RightArm");
 
        for (size_t i = 0; i < rightRNS->getSize(); ++i)
        {
            std::string jointName = rightRNS->getNode(i)->getName();
            rightJointNames.push_back(jointName);
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
 
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No velocitySensor available for " << jointName;
            }
            if (!positionSensor)
            {
                ARMARX_WARNING << "No positionSensor available for " << jointName;
            }
 
            rightVelocitySensors.push_back(velocitySensor);
            rightPositionSensors.push_back(positionSensor);
        };
 
 
        //        const SensorValueBase* svlf = prov->getSensorValue("FT L");
        const SensorValueBase* svlf = robUnit->getSensorDevice("FT L")->getSensorValue();
        leftForceTorque = svlf->asA<SensorValueForceTorque>();
        //        const SensorValueBase* svrf = prov->getSensorValue("FT R");
        const SensorValueBase* svrf = robUnit->getSensorDevice("FT R")->getSensorValue();
        rightForceTorque = svrf->asA<SensorValueForceTorque>();
 
        leftIK.reset(new VirtualRobot::DifferentialIK(
            leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
        rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS,
                                                       rightRNS->getRobot()->getRootNode(),
                                                       VirtualRobot::JacobiProvider::eSVDDamped));
 
 
        tsvmp::TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
        taskSpaceDMPConfig.motionTimeDuration = cfg->timeDuration;
        taskSpaceDMPConfig.DMPKernelSize = cfg->kernelSize;
        taskSpaceDMPConfig.DMPMode = cfg->dmpMode;
        taskSpaceDMPConfig.DMPStyle = cfg->dmpType;
        taskSpaceDMPConfig.DMPAmplitude = 1.0;
        taskSpaceDMPConfig.phaseStopParas.goDist = cfg->phaseDist0;
        taskSpaceDMPConfig.phaseStopParas.backDist = cfg->phaseDist1;
        taskSpaceDMPConfig.phaseStopParas.Kpos = 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;
 
 
        objectDMP.reset(new tsvmp::TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
        ARMARX_IMPORTANT << "dmp finieshed";
 
        tcpLeft = leftRNS->getTCP();
        tcpRight = rightRNS->getTCP();
 
        //initialize control parameters
        KpImpedance.setZero(cfg->KpImpedance.size());
        ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 12);
 
        for (int i = 0; i < KpImpedance.size(); ++i)
        {
            KpImpedance(i) = cfg->KpImpedance.at(i);
        }
 
        KdImpedance.setZero(cfg->KdImpedance.size());
        ARMARX_CHECK_EQUAL(cfg->KdImpedance.size(), 12);
 
        for (int i = 0; i < KdImpedance.size(); ++i)
        {
            KdImpedance(i) = cfg->KdImpedance.at(i);
        }
 
        KpAdmittance.setZero(cfg->KpAdmittance.size());
        ARMARX_CHECK_EQUAL(cfg->KpAdmittance.size(), 6);
 
        for (int i = 0; i < KpAdmittance.size(); ++i)
        {
            KpAdmittance(i) = cfg->KpAdmittance.at(i);
        }
 
        KdAdmittance.setZero(cfg->KdAdmittance.size());
        ARMARX_CHECK_EQUAL(cfg->KdAdmittance.size(), 6);
 
        for (int i = 0; i < KdAdmittance.size(); ++i)
        {
            KdAdmittance(i) = cfg->KdAdmittance.at(i);
        }
 
        KmAdmittance.setZero(cfg->KmAdmittance.size());
        ARMARX_CHECK_EQUAL(cfg->KmAdmittance.size(), 6);
 
        for (int i = 0; i < KmAdmittance.size(); ++i)
        {
            KmAdmittance(i) = cfg->KmAdmittance.at(i);
        }
 
 
        Inferface2rtData initInt2rtData;
        initInt2rtData.KpImpedance = KpImpedance;
        initInt2rtData.KdImpedance = KdImpedance;
        initInt2rtData.KmAdmittance = KmAdmittance;
        initInt2rtData.KpAdmittance = KpAdmittance;
        initInt2rtData.KdAdmittance = KdAdmittance;
        interface2rtBuffer.reinitAllBuffers(initInt2rtData);
 
        leftDesiredJointValues.resize(leftTargets.size());
        ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
 
        for (size_t i = 0; i < leftTargets.size(); ++i)
        {
            leftDesiredJointValues(i) = cfg->leftDesiredJointValues.at(i);
        }
 
        rightDesiredJointValues.resize(rightTargets.size());
        ARMARX_CHECK_EQUAL(cfg->rightDesiredJointValues.size(), rightTargets.size());
 
        for (size_t i = 0; i < rightTargets.size(); ++i)
        {
            rightDesiredJointValues(i) = cfg->rightDesiredJointValues.at(i);
        }
 
        KmPID.resize(cfg->KmPID.size());
        ARMARX_CHECK_EQUAL(cfg->KmPID.size(), 6);
 
        for (int i = 0; i < KmPID.size(); ++i)
        {
            KmPID(i) = cfg->KmPID.at(i);
        }
 
        virtualAcc.setZero(6);
        virtualVel.setZero(6);
        virtualPose = Eigen::Matrix4f::Identity();
        ARMARX_INFO << "got controller params";
 
 
        boxInitialPose = VirtualRobot::MathTools::quat2eigen4f(cfg->boxInitialPose[4],
                                                               cfg->boxInitialPose[5],
                                                               cfg->boxInitialPose[6],
                                                               cfg->boxInitialPose[3]);
        for (int i = 0; i < 3; ++i)
        {
            boxInitialPose(i, 3) = cfg->boxInitialPose[i];
        }
 
        rt2ControlData initSensorData;
        initSensorData.deltaT = 0;
        initSensorData.currentTime = 0;
        initSensorData.currentPose = boxInitialPose;
        initSensorData.currentTwist.setZero();
        rt2ControlBuffer.reinitAllBuffers(initSensorData);
 
 
        ControlInterfaceData initInterfaceData;
        initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
        initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
        initInterfaceData.currentObjPose = boxInitialPose;
        initInterfaceData.currentObjForce.setZero();
        initInterfaceData.currentObjVel.setZero();
        controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
 
 
        leftInitialPose = tcpLeft->getPoseInRootFrame();
        rightInitialPose = tcpRight->getPoseInRootFrame();
        leftInitialPose.block<3, 1>(0, 3) = 0.001 * leftInitialPose.block<3, 1>(0, 3);
        rightInitialPose.block<3, 1>(0, 3) = 0.001 * rightInitialPose.block<3, 1>(0, 3);
 
        //        leftInitialPose = boxInitialPose;
        //        leftInitialPose(0, 3) -= cfg->boxWidth * 0.5;
        //        rightInitialPose = boxInitialPose;
        //        rightInitialPose(0, 3) += cfg->boxWidth * 0.5;
 
        forcePIDControllers.resize(12);
        for (size_t i = 0; i < 6; i++)
        {
            forcePIDControllers.at(i).reset(new PIDController(
                cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
            forcePIDControllers.at(i + 6).reset(new PIDController(
                cfg->forceP[i], cfg->forceI[i], cfg->forceD[i], cfg->forcePIDLimits[i]));
            forcePIDControllers.at(i)->reset();
            forcePIDControllers.at(i + 6)->reset();
        }
 
        // filter
        filterCoeff = cfg->filterCoeff;
        ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
        filteredOldValue.setZero(12);
 
        // static compensation
        massLeft = cfg->massLeft;
        CoMVecLeft << cfg->CoMVecLeft[0], cfg->CoMVecLeft[1], cfg->CoMVecLeft[2];
        forceOffsetLeft << cfg->forceOffsetLeft[0], cfg->forceOffsetLeft[1],
            cfg->forceOffsetLeft[2];
        torqueOffsetLeft << cfg->torqueOffsetLeft[0], cfg->torqueOffsetLeft[1],
            cfg->torqueOffsetLeft[2];
 
        massRight = cfg->massRight;
        CoMVecRight << cfg->CoMVecRight[0], cfg->CoMVecRight[1], cfg->CoMVecRight[2];
        forceOffsetRight << cfg->forceOffsetRight[0], cfg->forceOffsetRight[1],
            cfg->forceOffsetRight[2];
        torqueOffsetRight << cfg->torqueOffsetRight[0], cfg->torqueOffsetRight[1],
            cfg->torqueOffsetRight[2];
 
        sensorFrame2TcpFrameLeft.setZero();
        sensorFrame2TcpFrameRight.setZero();
 
        NJointBimanualObjLevelControlData initData;
        initData.boxPose = boxInitialPose;
        initData.boxTwist.setZero(6);
        reinitTripleBuffer(initData);
 
        firstLoop = true;
        ARMARX_INFO << "left initial pose: \n"
                    << leftInitialPose << "\n right initial pose: \n"
                    << rightInitialPose;
 
        ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
        ARMARX_IMPORTANT << "finished construction!";
 
        dmpStarted = false;
 
 
        ftcalibrationTimer = 0;
        ftOffset.setZero(12);
 
        targetWrench.setZero(cfg->targetWrench.size());
        for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
        {
            targetWrench(i) = cfg->targetWrench[i];
        }
 
 
        fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
        objCom2TCPLeftInObjFrame.setZero();
        objCom2TCPRightInObjFrame.setZero();
    }
 
    void
    NJointBimanualObjLevelController::setMPWeights(const DoubleSeqSeq& weights, const Ice::Current&)
    {
        objectDMP->setWeights(weights);
    }
 
    DoubleSeqSeq
    NJointBimanualObjLevelController::getMPWeights(const Ice::Current&)
    {
        DMP::DVec2d res = objectDMP->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
    NJointBimanualObjLevelController::setMPRotWeights(const DoubleSeqSeq& weights,
                                                      const Ice::Current&)
    {
        objectDMP->setRotWeights(weights);
    }
 
    DoubleSeqSeq
    NJointBimanualObjLevelController::getMPRotWeights(const Ice::Current&)
    {
        DMP::DVec2d res = objectDMP->getRotWeights();
        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
    NJointBimanualObjLevelController::rtPreActivateController()
    {
        Eigen::Matrix4f boxInitPose = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
        Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
        leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) * 0.001;
        rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) * 0.001;
        boxInitPose.block<3, 1>(0, 3) =
            0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
        boxInitPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
 
        NJointBimanualObjLevelControlData initData;
        initData.boxPose = boxInitPose;
        initData.boxTwist.resize(6);
        reinitTripleBuffer(initData);
    }
 
    std::string
    NJointBimanualObjLevelController::getClassName(const Ice::Current&) const
    {
        return "NJointBimanualObjLevelController";
    }
 
    void
    NJointBimanualObjLevelController::controllerRun()
    {
        if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
        {
            return;
        }
 
        double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
        Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
        Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
        //ARMARX_IMPORTANT << "canVal:  " << objectDMP->canVal;
 
        if (objectDMP->canVal < 1e-8)
        {
            finished = true;
            dmpStarted = false;
        }
 
        objectDMP->flow(deltaT, currentPose, currentTwist);
 
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
        getWriterControlStruct().boxTwist = objectDMP->getTargetVelocity();
        writeControlStruct();
    }
 
    void
    NJointBimanualObjLevelController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                            const IceUtil::Time& timeSinceLastIteration)
    {
        Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
        Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
 
        controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
        controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
        double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        ftcalibrationTimer += deltaT;
 
        if (firstLoop)
        {
            Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
            Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
 
            leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3) * 0.001;
            rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3) * 0.001;
 
            virtualPose.block<3, 1>(0, 3) =
                0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
            virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
            fixedLeftRightRotOffset =
                virtualPose.block<3, 3>(0, 0).transpose() * rightPose.block<3, 3>(0, 0);
 
            Eigen::Vector3f objCom2TCPLeftInGlobal =
                leftPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
            Eigen::Vector3f objCom2TCPRightInGlobal =
                rightPose.block<3, 1>(0, 3) - virtualPose.block<3, 1>(0, 3);
            //        objCom2TCPLeft << -cfg->boxWidth * 0.5, 0, 0;
            //        objCom2TCPRight << cfg->boxWidth * 0.5, 0, 0;
 
            objCom2TCPLeftInObjFrame =
                virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
            objCom2TCPRightInObjFrame =
                virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
 
 
            Eigen::Matrix4f leftSensorFrame =
                rtGetRobot()->getSensor("FT L")->getRobotNode()->getPoseInRootFrame();
            Eigen::Matrix4f rightSensorFrame =
                rtGetRobot()->getSensor("FT R")->getRobotNode()->getPoseInRootFrame();
            leftSensorFrame.block<3, 1>(0, 3) = leftSensorFrame.block<3, 1>(0, 3) * 0.001;
            rightSensorFrame.block<3, 1>(0, 3) = rightSensorFrame.block<3, 1>(0, 3) * 0.001;
 
            sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) =
                leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
            sensorFrame2TcpFrameRight.block<3, 3>(0, 0) =
                rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
            CoMVecLeft = sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * CoMVecLeft;
            CoMVecRight = sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * CoMVecRight;
            firstLoop = false;
            ARMARX_INFO << "modified left pose:\n " << leftPose;
            ARMARX_INFO << "modified right pose:\n " << rightPose;
        }
 
        // --------------------------------------------- get control parameters ---------------------------------------
        KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
        KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
        KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
        KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
        KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
 
        if (ftcalibrationTimer < cfg->ftCalibrationTime)
        {
            ftOffset.block<3, 1>(0, 0) =
                0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
            ftOffset.block<3, 1>(3, 0) =
                0.5 * ftOffset.block<3, 1>(3, 0) + 0.5 * leftForceTorque->torque;
            ftOffset.block<3, 1>(6, 0) =
                0.5 * ftOffset.block<3, 1>(6, 0) + 0.5 * rightForceTorque->force;
            ftOffset.block<3, 1>(9, 0) =
                0.5 * ftOffset.block<3, 1>(9, 0) + 0.5 * rightForceTorque->torque;
 
            for (int i = 0; i < KmAdmittance.size(); ++i)
            {
                KmAdmittance(i) = 0;
            }
        }
        else
        {
            for (int i = 0; i < KmAdmittance.size(); ++i)
            {
                KmAdmittance(i) = cfg->KmAdmittance.at(i);
            }
        }
 
        // -------------------------------------------- target wrench ---------------------------------------------
        Eigen::VectorXf deltaPoseForWrenchControl;
        deltaPoseForWrenchControl.setZero(12);
        for (size_t i = 0; i < 12; ++i)
        {
            if (KpImpedance(i) == 0)
            {
                deltaPoseForWrenchControl(i) = 0;
            }
            else
            {
                deltaPoseForWrenchControl(i) = targetWrench(i) / KpImpedance(i);
                if (deltaPoseForWrenchControl(i) > 0.1)
                {
                    deltaPoseForWrenchControl(i) = 0.1;
                }
                else if (deltaPoseForWrenchControl(i) < -0.1)
                {
                    deltaPoseForWrenchControl(i) = -0.1;
                }
                //            deltaPoseForWrenchControl(i + 6) = targetWrench(i + 6) / KpImpedance(i);
            }
        }
 
        // ------------------------------------------- current tcp pose -------------------------------------------
 
        currentLeftPose.block<3, 1>(0, 3) = 0.001 * currentLeftPose.block<3, 1>(0, 3);
        currentRightPose.block<3, 1>(0, 3) = 0.001 * currentRightPose.block<3, 1>(0, 3);
 
        // --------------------------------------------- grasp matrix ---------------------------------------------
 
        Eigen::MatrixXf graspMatrix;
        graspMatrix.setZero(6, 12);
        graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
        graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
        //        graspMatrix.block<6, 6>(0, 0) = Eigen::MatrixXf::Identity(6, 6);
        //        graspMatrix.block<6, 6>(0, 6) = Eigen::MatrixXf::Identity(6, 6);
 
        Eigen::Vector3f rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPLeftInObjFrame;
        graspMatrix.block<3, 3>(3, 0) = skew(rvec);
 
        rvec = virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
        graspMatrix.block<3, 3>(3, 6) = skew(rvec);
 
        // // projection of grasp matrix
        // Eigen::MatrixXf pinvG = leftIK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
        // Eigen::MatrixXf G_range = pinvG * graspMatrix;
        // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
        float lambda = 1;
        Eigen::MatrixXf pinvGraspMatrixT =
            leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
 
        // ---------------------------------------------- object pose ----------------------------------------------
        Eigen::Matrix4f boxCurrentPose = currentRightPose;
        boxCurrentPose.block<3, 1>(0, 3) =
            0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
        Eigen::VectorXf boxCurrentTwist;
        boxCurrentTwist.setZero(6);
 
        // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(leftTargets.size(), leftTargets.size());
        // Jacobian matrices
        Eigen::MatrixXf jacobiL =
            leftIK->getJacobianMatrix(tcpLeft, VirtualRobot::IKSolver::CartesianSelection::All);
        Eigen::MatrixXf jacobiR =
            rightIK->getJacobianMatrix(tcpRight, VirtualRobot::IKSolver::CartesianSelection::All);
        jacobiL.block<3, 8>(0, 0) = 0.001 * jacobiL.block<3, 8>(0, 0);
        jacobiR.block<3, 8>(0, 0) = 0.001 * jacobiR.block<3, 8>(0, 0);
 
        // qpos, qvel
        Eigen::VectorXf leftqpos;
        Eigen::VectorXf leftqvel;
        leftqpos.resize(leftPositionSensors.size());
        leftqvel.resize(leftVelocitySensors.size());
        for (size_t i = 0; i < leftVelocitySensors.size(); ++i)
        {
            leftqpos(i) = leftPositionSensors[i]->position;
            leftqvel(i) = leftVelocitySensors[i]->velocity;
        }
 
        Eigen::VectorXf rightqpos;
        Eigen::VectorXf rightqvel;
        rightqpos.resize(rightPositionSensors.size());
        rightqvel.resize(rightVelocitySensors.size());
 
        for (size_t i = 0; i < rightVelocitySensors.size(); ++i)
        {
            rightqpos(i) = rightPositionSensors[i]->position;
            rightqvel(i) = rightVelocitySensors[i]->velocity;
        }
 
        // -------------------------------------- compute TCP and object velocity -------------------------------------
        Eigen::VectorXf currentLeftTwist = jacobiL * leftqvel;
        Eigen::VectorXf currentRightTwist = jacobiR * rightqvel;
 
        Eigen::VectorXf currentTwist(12);
        currentTwist << currentLeftTwist, currentRightTwist;
        boxCurrentTwist = pinvGraspMatrixT * currentTwist;
 
        rt2ControlBuffer.getWriteBuffer().currentPose = boxCurrentPose;
        rt2ControlBuffer.getWriteBuffer().currentTwist = boxCurrentTwist;
        rt2ControlBuffer.getWriteBuffer().deltaT = deltaT;
        rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
        rt2ControlBuffer.commitWrite();
 
 
        // --------------------------------------------- get ft sensor ---------------------------------------------
        // static compensation
        Eigen::Vector3f gravity;
        gravity << 0.0, 0.0, -9.8;
        Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
        Eigen::Vector3f localForceVecLeft = massLeft * localGravityLeft;
        Eigen::Vector3f localTorqueVecLeft = CoMVecLeft.cross(localForceVecLeft);
 
        Eigen::Vector3f localGravityRight =
            currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
        Eigen::Vector3f localForceVecRight = massRight * localGravityRight;
        Eigen::Vector3f localTorqueVecRight = CoMVecRight.cross(localForceVecRight);
 
        // mapping of measured wrenches
        Eigen::VectorXf wrenchesMeasured(12);
        wrenchesMeasured << leftForceTorque->force - forceOffsetLeft,
            leftForceTorque->torque - torqueOffsetLeft, rightForceTorque->force - forceOffsetRight,
            rightForceTorque->torque - torqueOffsetRight;
        for (size_t i = 0; i < 12; i++)
        {
            wrenchesMeasured(i) =
                (1 - filterCoeff) * wrenchesMeasured(i) + filterCoeff * filteredOldValue(i);
        }
        filteredOldValue = wrenchesMeasured;
        wrenchesMeasured.block<3, 1>(0, 0) =
            sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) -
            localForceVecLeft;
        wrenchesMeasured.block<3, 1>(3, 0) =
            sensorFrame2TcpFrameLeft.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) -
            localTorqueVecLeft;
        wrenchesMeasured.block<3, 1>(6, 0) =
            sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) -
            localForceVecRight;
        wrenchesMeasured.block<3, 1>(9, 0) =
            sensorFrame2TcpFrameRight.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) -
            localTorqueVecRight;
        //        if (wrenchesMeasured.norm() < cfg->forceThreshold)
        //        {
        //            wrenchesMeasured.setZero();
        //        }
 
        Eigen::VectorXf wrenchesMeasuredInRoot(12);
        wrenchesMeasuredInRoot.block<3, 1>(0, 0) =
            currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0);
        wrenchesMeasuredInRoot.block<3, 1>(3, 0) =
            currentLeftPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0);
        wrenchesMeasuredInRoot.block<3, 1>(6, 0) =
            currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0);
        wrenchesMeasuredInRoot.block<3, 1>(9, 0) =
            currentRightPose.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0);
 
 
        // map to object
        Eigen::VectorXf objFTValue = graspMatrix * wrenchesMeasuredInRoot;
        for (size_t i = 0; i < 6; i++)
        {
            if (fabs(objFTValue(i)) < cfg->forceThreshold.at(i))
            {
                objFTValue(i) = 0;
            }
            else
            {
                objFTValue(i) -= cfg->forceThreshold.at(i) * objFTValue(i) / fabs(objFTValue(i));
            }
        }
 
        // pass sensor value to statechart
        controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
        controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head<3>();
        controlInterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head<3>();
        controlInterfaceBuffer.commitWrite();
 
 
        // --------------------------------------------- get MP target ---------------------------------------------
        Eigen::Matrix4f boxPose = rtGetControlStruct().boxPose;
        Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
 
 
        // --------------------------------------------- obj admittance control ---------------------------------------------
        // admittance
        Eigen::VectorXf objPoseError(6);
        objPoseError.head<3>() = virtualPose.block<3, 1>(0, 3) - boxPose.block<3, 1>(0, 3);
        Eigen::Matrix3f objDiffMat =
            virtualPose.block<3, 3>(0, 0) * boxPose.block<3, 3>(0, 0).transpose();
        objPoseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
 
 
        Eigen::VectorXf objAcc;
        Eigen::VectorXf objVel;
        objAcc.setZero(6);
        objVel.setZero(6);
        for (size_t i = 0; i < 6; i++)
        {
            //            objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) - KdAdmittance(i) * (virtualVel(i) - boxTwist(i));
            objAcc(i) = KmAdmittance(i) * objFTValue(i) - KpAdmittance(i) * objPoseError(i) -
                        KdAdmittance(i) * virtualVel(i);
        }
        objVel = virtualVel + 0.5 * deltaT * (objAcc + virtualAcc);
        Eigen::VectorXf deltaObjPose = 0.5 * deltaT * (objVel + virtualVel);
        virtualAcc = objAcc;
        virtualVel = objVel;
        virtualPose.block<3, 1>(0, 3) += deltaObjPose.head<3>();
        virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(
                                            deltaObjPose(3), deltaObjPose(4), deltaObjPose(5)) *
                                        virtualPose.block<3, 3>(0, 0);
 
        // --------------------------------------------- convert to tcp pose ---------------------------------------------
        Eigen::Matrix4f tcpTargetPoseLeft = virtualPose;
        Eigen::Matrix4f tcpTargetPoseRight = virtualPose;
 
        tcpTargetPoseRight.block<3, 3>(0, 0) =
            virtualPose.block<3, 3>(0, 0) * fixedLeftRightRotOffset;
 
        tcpTargetPoseLeft.block<3, 1>(0, 3) +=
            virtualPose.block<3, 3>(0, 0) *
            (objCom2TCPLeftInObjFrame - deltaPoseForWrenchControl.block<3, 1>(0, 0));
        tcpTargetPoseRight.block<3, 1>(0, 3) +=
            virtualPose.block<3, 3>(0, 0) *
            (objCom2TCPRightInObjFrame - deltaPoseForWrenchControl.block<3, 1>(6, 0));
 
        // --------------------------------------------- Impedance control ---------------------------------------------
        Eigen::VectorXf poseError(12);
        Eigen::Matrix3f diffMat =
            tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).transpose();
        poseError.block<3, 1>(0, 0) =
            tcpTargetPoseLeft.block<3, 1>(0, 3) - currentLeftPose.block<3, 1>(0, 3);
        poseError.block<3, 1>(3, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
 
        diffMat =
            tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).transpose();
        poseError.block<3, 1>(6, 0) =
            tcpTargetPoseRight.block<3, 1>(0, 3) - currentRightPose.block<3, 1>(0, 3);
        poseError.block<3, 1>(9, 0) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
 
        Eigen::VectorXf forceImpedance(12);
        for (size_t i = 0; i < 12; i++)
        {
            forceImpedance(i) = KpImpedance(i) * poseError(i) - KdImpedance(i) * currentTwist(i);
            //            forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
        }
 
        // --------------------------------------------- Nullspace control ---------------------------------------------
        Eigen::VectorXf leftNullspaceTorque =
            cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel;
        Eigen::VectorXf rightNullspaceTorque =
            cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel;
 
        // --------------------------------------------- Set Torque Control Command ---------------------------------------------
        //        float lambda = 1;
        Eigen::MatrixXf jtpinvL =
            leftIK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
        Eigen::MatrixXf jtpinvR =
            rightIK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
        Eigen::VectorXf leftJointDesiredTorques =
            jacobiL.transpose() * forceImpedance.head<6>() +
            (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
        Eigen::VectorXf rightJointDesiredTorques =
            jacobiR.transpose() * forceImpedance.tail<6>() +
            (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
 
        // torque limit
        for (size_t i = 0; i < leftTargets.size(); ++i)
        {
            float desiredTorque = leftJointDesiredTorques(i);
            if (isnan(desiredTorque))
            {
                desiredTorque = 0;
            }
            desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
            desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
            debugOutputData.getWriteBuffer().desired_torques[leftJointNames[i]] =
                leftJointDesiredTorques(i);
            leftTargets.at(i)->torque = desiredTorque;
        }
 
        for (size_t i = 0; i < rightTargets.size(); ++i)
        {
            float desiredTorque = rightJointDesiredTorques(i);
            if (isnan(desiredTorque))
            {
                desiredTorque = 0;
            }
            desiredTorque = (desiredTorque > cfg->torqueLimit) ? cfg->torqueLimit : desiredTorque;
            desiredTorque = (desiredTorque < -cfg->torqueLimit) ? -cfg->torqueLimit : desiredTorque;
            debugOutputData.getWriteBuffer().desired_torques[rightJointNames[i]] =
                rightJointDesiredTorques(i);
            rightTargets.at(i)->torque = desiredTorque;
        }
 
        // --------------------------------------------- debug output ---------------------------------------------
        debugOutputData.getWriteBuffer().forceImpedance = forceImpedance;
        debugOutputData.getWriteBuffer().poseError = poseError;
        //        debugOutputData.getWriteBuffer().wrenchesConstrained = wrenchesConstrained;
        debugOutputData.getWriteBuffer().wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
        //        debugOutputData.getWriteBuffer().wrenchDMP = wrenchDMP;
        //        debugOutputData.getWriteBuffer().computedBoxWrench = computedBoxWrench;
 
        debugOutputData.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
        debugOutputData.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
        debugOutputData.getWriteBuffer().virtualPose_z = virtualPose(2, 3);
 
        debugOutputData.getWriteBuffer().objPose_x = boxCurrentPose(0, 3);
        debugOutputData.getWriteBuffer().objPose_y = boxCurrentPose(1, 3);
        debugOutputData.getWriteBuffer().objPose_z = boxCurrentPose(2, 3);
 
        debugOutputData.getWriteBuffer().objForce_x = objFTValue(0);
        debugOutputData.getWriteBuffer().objForce_y = objFTValue(1);
        debugOutputData.getWriteBuffer().objForce_z = objFTValue(2);
        debugOutputData.getWriteBuffer().objTorque_x = objFTValue(3);
        debugOutputData.getWriteBuffer().objTorque_y = objFTValue(4);
        debugOutputData.getWriteBuffer().objTorque_z = objFTValue(5);
 
        debugOutputData.getWriteBuffer().objVel_x = objVel(0);
        debugOutputData.getWriteBuffer().objVel_y = objVel(1);
        debugOutputData.getWriteBuffer().objVel_z = objVel(2);
        debugOutputData.getWriteBuffer().objVel_rx = objVel(3);
        debugOutputData.getWriteBuffer().objVel_ry = objVel(4);
        debugOutputData.getWriteBuffer().objVel_rz = objVel(5);
 
        debugOutputData.getWriteBuffer().deltaPose_x = deltaObjPose(0);
        debugOutputData.getWriteBuffer().deltaPose_y = deltaObjPose(1);
        debugOutputData.getWriteBuffer().deltaPose_z = deltaObjPose(2);
        debugOutputData.getWriteBuffer().deltaPose_rx = deltaObjPose(3);
        debugOutputData.getWriteBuffer().deltaPose_ry = deltaObjPose(4);
        debugOutputData.getWriteBuffer().deltaPose_rz = deltaObjPose(5);
 
        debugOutputData.getWriteBuffer().modifiedPoseRight_x = tcpTargetPoseRight(0, 3);
        debugOutputData.getWriteBuffer().modifiedPoseRight_y = tcpTargetPoseRight(1, 3);
        debugOutputData.getWriteBuffer().modifiedPoseRight_z = tcpTargetPoseRight(2, 3);
 
        debugOutputData.getWriteBuffer().currentPoseLeft_x = currentLeftPose(0, 3);
        debugOutputData.getWriteBuffer().currentPoseLeft_y = currentLeftPose(1, 3);
        debugOutputData.getWriteBuffer().currentPoseLeft_z = currentLeftPose(2, 3);
 
 
        VirtualRobot::MathTools::Quaternion leftQuat =
            VirtualRobot::MathTools::eigen4f2quat(currentLeftPose);
        debugOutputData.getWriteBuffer().leftQuat_w = leftQuat.w;
        debugOutputData.getWriteBuffer().leftQuat_x = leftQuat.x;
        debugOutputData.getWriteBuffer().leftQuat_y = leftQuat.y;
        debugOutputData.getWriteBuffer().leftQuat_z = leftQuat.y;
 
        debugOutputData.getWriteBuffer().modifiedPoseLeft_x = tcpTargetPoseLeft(0, 3);
        debugOutputData.getWriteBuffer().modifiedPoseLeft_y = tcpTargetPoseLeft(1, 3);
        debugOutputData.getWriteBuffer().modifiedPoseLeft_z = tcpTargetPoseLeft(2, 3);
 
        debugOutputData.getWriteBuffer().currentPoseRight_x = currentRightPose(0, 3);
        debugOutputData.getWriteBuffer().currentPoseRight_y = currentRightPose(1, 3);
        debugOutputData.getWriteBuffer().currentPoseRight_z = currentRightPose(2, 3);
 
        VirtualRobot::MathTools::Quaternion rightQuat =
            VirtualRobot::MathTools::eigen4f2quat(currentRightPose);
        debugOutputData.getWriteBuffer().rightQuat_w = rightQuat.w;
        debugOutputData.getWriteBuffer().rightQuat_x = rightQuat.x;
        debugOutputData.getWriteBuffer().rightQuat_y = rightQuat.y;
        debugOutputData.getWriteBuffer().rightQuat_z = rightQuat.y;
 
 
        debugOutputData.getWriteBuffer().dmpBoxPose_x = boxPose(0, 3);
        debugOutputData.getWriteBuffer().dmpBoxPose_y = boxPose(1, 3);
        debugOutputData.getWriteBuffer().dmpBoxPose_z = boxPose(2, 3);
 
        debugOutputData.getWriteBuffer().dmpTwist_x = boxTwist(0);
        debugOutputData.getWriteBuffer().dmpTwist_y = boxTwist(1);
        debugOutputData.getWriteBuffer().dmpTwist_z = boxTwist(2);
        debugOutputData.getWriteBuffer().rx = rvec(0);
        debugOutputData.getWriteBuffer().ry = rvec(1);
        debugOutputData.getWriteBuffer().rz = rvec(2);
 
        //        debugOutputData.getWriteBuffer().modifiedTwist_lx = twistDMP(0);
        //        debugOutputData.getWriteBuffer().modifiedTwist_ly = twistDMP(1);
        //        debugOutputData.getWriteBuffer().modifiedTwist_lz = twistDMP(2);
        //        debugOutputData.getWriteBuffer().modifiedTwist_rx = twistDMP(6);
        //        debugOutputData.getWriteBuffer().modifiedTwist_ry = twistDMP(7);
        //        debugOutputData.getWriteBuffer().modifiedTwist_rz = twistDMP(8);
 
        //        debugOutputData.getWriteBuffer().forcePID = forcePIDInRootForDebug;
 
        debugOutputData.commitWrite();
    }
 
    void
    NJointBimanualObjLevelController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
                                                        const Ice::Current&)
    {
        objectDMP->learnDMPFromFiles(fileNames);
    }
 
    void
    NJointBimanualObjLevelController::setGoals(const Ice::DoubleSeq& goals, const Ice::Current& ice)
    {
        LockGuardType guard(controllerMutex);
        objectDMP->setGoalPoseVec(goals);
    }
 
    void
    NJointBimanualObjLevelController::runDMP(const Ice::DoubleSeq& goals,
                                             Ice::Double timeDuration,
                                             const Ice::Current&)
    {
        while (!controlInterfaceBuffer.updateReadBuffer())
        {
            usleep(1000);
        }
 
        Eigen::Matrix4f leftPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentLeftPose;
        Eigen::Matrix4f rightPose = controlInterfaceBuffer.getUpToDateReadBuffer().currentRightPose;
 
        VirtualRobot::MathTools::Quaternion boxOri =
            VirtualRobot::MathTools::eigen4f2quat(leftPose);
        Eigen::Vector3f boxPosi = (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3)) / 2;
 
        ARMARX_IMPORTANT << "runDMP: boxPosi: " << boxPosi;
 
        std::vector<double> boxInitialPose;
        for (size_t i = 0; i < 3; ++i)
        {
            boxInitialPose.push_back(boxPosi(i) * 0.001); //Important: mm -> m
        }
        boxInitialPose.push_back(boxOri.w);
        boxInitialPose.push_back(boxOri.x);
        boxInitialPose.push_back(boxOri.y);
        boxInitialPose.push_back(boxOri.z);
 
        objectDMP->prepareExecution(boxInitialPose, goals);
        objectDMP->canVal = timeDuration;
        objectDMP->config.motionTimeDuration = timeDuration;
 
 
        finished = false;
        dmpStarted = true;
    }
 
    void
    NJointBimanualObjLevelController::runDMPWithVirtualStart(const Ice::DoubleSeq& starts,
                                                             const Ice::DoubleSeq& goals,
                                                             Ice::Double timeDuration,
                                                             const Ice::Current&)
    {
        while (!controlInterfaceBuffer.updateReadBuffer())
        {
            usleep(1000);
        }
        ARMARX_IMPORTANT << "obj level control: setup dmp ...";
        objectDMP->prepareExecution(starts, goals);
        objectDMP->canVal = timeDuration;
        objectDMP->config.motionTimeDuration = timeDuration;
 
        finished = false;
        dmpStarted = true;
 
        ARMARX_IMPORTANT << "obj level control: run dmp with virtual start.";
    }
 
    void
    NJointBimanualObjLevelController::setViaPoints(Ice::Double u,
                                                   const Ice::DoubleSeq& viapoint,
                                                   const Ice::Current&)
    {
        //        LockGuardType guard(controllerMutex);
        ARMARX_INFO << "setting via points ";
        objectDMP->setViaPose(u, viapoint);
    }
 
    void
    NJointBimanualObjLevelController::removeAllViaPoints(const Ice::Current&)
    {
        objectDMP->removeAllViaPoints();
    }
 
    void
    NJointBimanualObjLevelController::setKpImpedance(const Ice::FloatSeq& value,
                                                     const Ice::Current&)
    {
 
        Eigen::VectorXf setpoint;
        setpoint.setZero(value.size());
        ARMARX_CHECK_EQUAL(value.size(), 12);
 
        for (size_t i = 0; i < value.size(); ++i)
        {
            setpoint(i) = value.at(i);
        }
 
        LockGuardType guard{interfaceDataMutex};
        interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
        interface2rtBuffer.commitWrite();
    }
 
    void
    NJointBimanualObjLevelController::setKdImpedance(const Ice::FloatSeq& value,
                                                     const Ice::Current&)
    {
        Eigen::VectorXf setpoint;
        setpoint.setZero(value.size());
        ARMARX_CHECK_EQUAL(value.size(), 12);
 
        for (size_t i = 0; i < value.size(); ++i)
        {
            setpoint(i) = value.at(i);
        }
 
        LockGuardType guard{interfaceDataMutex};
        interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
        interface2rtBuffer.commitWrite();
    }
 
    void
    NJointBimanualObjLevelController::setKpAdmittance(const Ice::FloatSeq& value,
                                                      const Ice::Current&)
    {
        Eigen::VectorXf setpoint;
        setpoint.setZero(value.size());
        ARMARX_CHECK_EQUAL(value.size(), 6);
 
        for (size_t i = 0; i < value.size(); ++i)
        {
            setpoint(i) = value.at(i);
        }
 
        LockGuardType guard{interfaceDataMutex};
        interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
        interface2rtBuffer.commitWrite();
    }
 
    void
    NJointBimanualObjLevelController::setKdAdmittance(const Ice::FloatSeq& value,
                                                      const Ice::Current&)
    {
        Eigen::VectorXf setpoint;
        setpoint.setZero(value.size());
        ARMARX_CHECK_EQUAL(value.size(), 6);
 
        for (size_t i = 0; i < value.size(); ++i)
        {
            setpoint(i) = value.at(i);
        }
 
        LockGuardType guard{interfaceDataMutex};
        interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
        interface2rtBuffer.commitWrite();
    }
 
    std::vector<float>
    NJointBimanualObjLevelController::getCurrentObjVel(const Ice::Current&)
    {
        Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
        std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
        return tvelvec;
    }
 
    std::vector<float>
    NJointBimanualObjLevelController::getCurrentObjForce(const Ice::Current&)
    {
        Eigen::Vector3f fvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
        std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
        return fvelvec;
    }
 
    void
    NJointBimanualObjLevelController::setKmAdmittance(const Ice::FloatSeq& value,
                                                      const Ice::Current&)
    {
        Eigen::VectorXf setpoint;
        setpoint.setZero(value.size());
        ARMARX_CHECK_EQUAL(value.size(), 6);
 
        for (size_t i = 0; i < value.size(); ++i)
        {
            setpoint(i) = value.at(i);
        }
 
        LockGuardType guard{interfaceDataMutex};
        interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
        interface2rtBuffer.commitWrite();
    }
 
    void
    NJointBimanualObjLevelController::onPublish(const SensorAndControl&,
                                                const DebugDrawerInterfacePrx&,
                                                const DebugObserverInterfacePrx& debugObs)
    {
 
        StringVariantBaseMap datafields;
        auto values = debugOutputData.getUpToDateReadBuffer().desired_torques;
        for (auto& pair : values)
        {
            datafields[pair.first] = new Variant(pair.second);
        }
 
        Eigen::VectorXf forceImpedance = debugOutputData.getUpToDateReadBuffer().forceImpedance;
        for (int i = 0; i < forceImpedance.rows(); ++i)
        {
            std::stringstream ss;
            ss << i;
            std::string data_name = "forceImpedance_" + ss.str();
            datafields[data_name] = new Variant(forceImpedance(i));
        }
 
        Eigen::VectorXf forcePID = debugOutputData.getUpToDateReadBuffer().forcePID;
        for (int i = 0; i < forcePID.rows(); ++i)
        {
            std::stringstream ss;
            ss << i;
            std::string data_name = "forcePID_" + ss.str();
            datafields[data_name] = new Variant(forcePID(i));
        }
 
 
        Eigen::VectorXf poseError = debugOutputData.getUpToDateReadBuffer().poseError;
        for (int i = 0; i < poseError.rows(); ++i)
        {
            std::stringstream ss;
            ss << i;
            std::string data_name = "poseError_" + ss.str();
            datafields[data_name] = new Variant(poseError(i));
        }
 
        Eigen::VectorXf wrenchesConstrained =
            debugOutputData.getUpToDateReadBuffer().wrenchesConstrained;
        for (int i = 0; i < wrenchesConstrained.rows(); ++i)
        {
            std::stringstream ss;
            ss << i;
            std::string data_name = "wrenchesConstrained_" + ss.str();
            datafields[data_name] = new Variant(wrenchesConstrained(i));
        }
 
        Eigen::VectorXf wrenchesMeasuredInRoot =
            debugOutputData.getUpToDateReadBuffer().wrenchesMeasuredInRoot;
        for (int i = 0; i < wrenchesMeasuredInRoot.rows(); ++i)
        {
            std::stringstream ss;
            ss << i;
            std::string data_name = "wrenchesMeasuredInRoot_" + ss.str();
            datafields[data_name] = new Variant(wrenchesMeasuredInRoot(i));
        }
 
 
        //        Eigen::VectorXf wrenchDMP = debugOutputData.getUpToDateReadBuffer().wrenchDMP;
        //        for (size_t i = 0; i < wrenchDMP.rows(); ++i)
        //        {
        //            std::stringstream ss;
        //            ss << i;
        //            std::string data_name = "wrenchDMP_" + ss.str();
        //            datafields[data_name] = new Variant(wrenchDMP(i));
        //        }
 
        //        Eigen::VectorXf computedBoxWrench = debugOutputData.getUpToDateReadBuffer().computedBoxWrench;
        //        for (size_t i = 0; i < computedBoxWrench.rows(); ++i)
        //        {
        //            std::stringstream ss;
        //            ss << i;
        //            std::string data_name = "computedBoxWrench_" + ss.str();
        //            datafields[data_name] = new Variant(computedBoxWrench(i));
        //        }
 
 
        datafields["virtualPose_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_x);
        datafields["virtualPose_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_y);
        datafields["virtualPose_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().virtualPose_z);
 
        datafields["objPose_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_x);
        datafields["objPose_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_y);
        datafields["objPose_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objPose_z);
 
        datafields["objForce_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_x);
        datafields["objForce_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_y);
        datafields["objForce_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objForce_z);
        datafields["objTorque_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_x);
        datafields["objTorque_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_y);
        datafields["objTorque_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().objTorque_z);
 
        datafields["objVel_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_x);
        datafields["objVel_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_y);
        datafields["objVel_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_z);
        datafields["objVel_rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_rx);
        datafields["objVel_ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_ry);
        datafields["objVel_rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().objVel_rz);
 
        datafields["deltaPose_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_x);
        datafields["deltaPose_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_y);
        datafields["deltaPose_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_z);
        datafields["deltaPose_rx"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_rx);
        datafields["deltaPose_ry"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_ry);
        datafields["deltaPose_rz"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().deltaPose_rz);
 
        datafields["modifiedPoseRight_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_x);
        datafields["modifiedPoseRight_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_y);
        datafields["modifiedPoseRight_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseRight_z);
        datafields["currentPoseLeft_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_x);
        datafields["currentPoseLeft_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_y);
        datafields["currentPoseLeft_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseLeft_z);
 
        datafields["leftQuat_w"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_w);
        datafields["leftQuat_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_x);
        datafields["leftQuat_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_y);
        datafields["leftQuat_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().leftQuat_z);
        datafields["rightQuat_w"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_w);
        datafields["rightQuat_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_x);
        datafields["rightQuat_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_y);
        datafields["rightQuat_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().rightQuat_z);
 
        datafields["modifiedPoseLeft_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_x);
        datafields["modifiedPoseLeft_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_y);
        datafields["modifiedPoseLeft_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedPoseLeft_z);
 
        datafields["currentPoseRight_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_x);
        datafields["currentPoseRight_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_y);
        datafields["currentPoseRight_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().currentPoseRight_z);
        datafields["dmpBoxPose_x"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_x);
        datafields["dmpBoxPose_y"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_y);
        datafields["dmpBoxPose_z"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_z);
        datafields["dmpTwist_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_x);
        datafields["dmpTwist_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_y);
        datafields["dmpTwist_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().dmpTwist_z);
 
        datafields["modifiedTwist_lx"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lx);
        datafields["modifiedTwist_ly"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ly);
        datafields["modifiedTwist_lz"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_lz);
        datafields["modifiedTwist_rx"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rx);
        datafields["modifiedTwist_ry"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_ry);
        datafields["modifiedTwist_rz"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().modifiedTwist_rz);
 
        datafields["rx"] = new Variant(debugOutputData.getUpToDateReadBuffer().rx);
        datafields["ry"] = new Variant(debugOutputData.getUpToDateReadBuffer().ry);
        datafields["rz"] = new Variant(debugOutputData.getUpToDateReadBuffer().rz);
 
 
        debugObs->setDebugChannel("BimanualForceController", datafields);
    }
 
    void
    NJointBimanualObjLevelController::onInitNJointController()
    {
 
 
        ARMARX_INFO << "init ...";
        runTask("NJointBimanualObjLevelController",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            controllerRun();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    NJointBimanualObjLevelController::onDisconnectNJointController()
    {
        ARMARX_INFO << "stopped ...";
    }
} // namespace armarx::control::deprecated_njoint_mp_controller::bimanual