NJointBimanualCartesianAdmittanceController.cpp

Go to the documentation of this file.

#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/math/pose/is_homogeneous_transform.h>
#include <SimoxUtility/math/isfinite.h>
 
#include <ArmarXCore/core/time/CycleUtil.h>
 
#include "NJointBimanualCartesianAdmittanceController.h"
 
namespace armarx::control::deprecated_njoint_mp_controller::bimanual
{
    NJointControllerRegistration<NJointBimanualCartesianAdmittanceController> registrationControllerNJointBimanualCartesianAdmittanceController("NJointBimanualCartesianAdmittanceController");
 
    NJointBimanualCartesianAdmittanceController::NJointBimanualCartesianAdmittanceController(const RobotUnitPtr& robUnit, const armarx::NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
    {
        ARMARX_INFO << "Preparing ... bimanual ";
        useSynchronizedRtRobot();
        auto cfgPtr = NJointBimanualCartesianAdmittanceControllerConfigPtr::dynamicCast(config);
        ARMARX_CHECK_NOT_NULL(cfgPtr);
        //init rtData
        {
            auto initRtData = [&](auto & data, const auto & rnsName, const auto & ftName, const auto & ftRefFrame)
            {
                data.rns = rtGetRobot()->getRobotNodeSet(rnsName);
                ARMARX_CHECK_NOT_NULL(data.rns) << "No robot node set " << rnsName;
                data.tcp = data.rns->getTCP();
                ARMARX_CHECK_NOT_NULL(data.tcp) << "No TCP in robot node set " << rnsName;
                data.frameFTSensor = rtGetRobot()->getSensor(ftName)->getRobotNode();
                ARMARX_CHECK_NOT_NULL(data.frameFTSensor) << "No ref frame for ft sensor in robot ";
 
                for (size_t i = 0; i < data.rns->getSize(); ++i)
                {
                    std::string jointName = data.rns->getNode(i)->getName();
                    data.jointNames.push_back(jointName);
                    ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
                    const SensorValueBase* sv = useSensorValue(jointName);
                    ARMARX_CHECK_NOT_NULL(ct) << "No control target available for " << jointName;
                    ARMARX_CHECK_NOT_NULL(sv) << "No sensor value available for " << jointName;
                    data.targets.push_back(ct->asA<ControlTarget1DoFActuatorTorque>());
                    const SensorValue1DoFActuatorVelocity* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
                    const SensorValue1DoFActuatorPosition* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
                    ARMARX_CHECK_NOT_NULL(velocitySensor) << "No velocitySensor available for " << jointName;
                    ARMARX_CHECK_NOT_NULL(positionSensor) << "No positionSensor available for " << jointName;
                    data.velocitySensors.push_back(velocitySensor);
                    data.positionSensors.push_back(positionSensor);
                }
                const auto ftDev = robUnit->getSensorDevice(ftName);
                ARMARX_CHECK_NOT_NULL(ftDev) << "No sensor device available for " << ftName;
                const SensorValueBase* svlf = ftDev->getSensorValue();
                ARMARX_CHECK_NOT_NULL(svlf) << "No sensor value available for " << ftName;
                data.forceTorque = svlf->asA<SensorValueForceTorque>();
                ARMARX_CHECK_NOT_NULL(data.forceTorque) << "Sensor value for " << ftName << " is not of type SensorValueForceTorque";
                data.IK.reset(new VirtualRobot::DifferentialIK(
                                  data.rns,
                                  data.rns->getRobot()->getRootNode(),
                                  VirtualRobot::JacobiProvider::eSVDDamped));
            };
 
            initRtData(rt.left, cfgPtr->kinematicChainLeft, cfgPtr->ftSensorLeft, cfgPtr->ftSensorLeftFrame);
            initRtData(rt.right, cfgPtr->kinematicChainRight, cfgPtr->ftSensorRight, cfgPtr->ftSensorRightFrame);
        }
 
        //init cfg + check it
        {
            setConfig(cfgPtr);
            cfgBuf.reinitAllBuffers(cfgBuf.getWriteBuffer());
        }
 
 
        //        {
        //            rt2ControlData initSensorData;
        //            initSensorData.deltaT = 0;
        //            initSensorData.currentTime = 0;
        //            initSensorData.currentPose = boxInitialPose;
        //            initSensorData.currentTwist.setZero();
        //            rt2ControlBuffer.reinitAllBuffers(initSensorData);
        //        }
 
 
        //        {
        //            ControlInterfaceData initInterfaceData;
        //            initInterfaceData.currentLeftPose = rt.left.tcp->getPoseInRootFrame();
        //            initInterfaceData.currentRightPose = rt.right.tcp->getPoseInRootFrame();
        //            controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
        //        }
 
        //////////////////////////////TODO
        //        leftInitialPose = rt.left.tcp->getPoseInRootFrame();
        //        rightInitialPose = rt.right.rns->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;
        //////////////////////////////TODO
        //        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();
        //        }
        //////////////////////////////TODO
        // filter
        //        filterCoeff = cfg->filterCoeff;
        //        ARMARX_IMPORTANT << "filter coeff.: " << filterCoeff;
        //        filteredOldValue.setZero(12);
 
        // static compensation
        rt.left.sensorFrame2TcpFrame.setZero();
        rt.right.sensorFrame2TcpFrame.setZero();
        //        NJointBimanualObjLevelControlData initData;
        //        initData.boxPose = boxInitialPose;
        //        initData.boxTwist.setZero(6);
        //        reinitTripleBuffer(initData);
 
        //        ARMARX_INFO << "left initial pose: \n" << leftInitialPose  << "\n right initial pose: \n" << rightInitialPose;
 
        //        ARMARX_IMPORTANT << "targetwrench is: " << cfg->targetWrench;
        ARMARX_IMPORTANT << "finished construction!";
 
        //        targetWrench.setZero(cfg->targetWrench.size());
        //        for (size_t i = 0; i < cfg->targetWrench.size(); ++i)
        //        {
        //            targetWrench(i) = cfg->targetWrench[i];
        //        }
    }
 
 
    void NJointBimanualCartesianAdmittanceController::rtPreActivateController()
    {
        //        NJointBimanualObjLevelControlData initData;
        //        initData.boxPose = boxInitPose;
        //        initData.boxTwist.resize(6);
        //        reinitTripleBuffer(initData);
 
        rt.virtualAcc.setZero();
        rt.virtualVel.setZero();
        rt.virtualPose.setZero();
        rt.filteredOldValue.setZero();
        //        rt.ftOffset.setZero();
        rt.firstLoop = true;
        rt.ftcalibrationTimer = 0;
 
 
 
        const Eigen::Matrix4f leftPose = simox::math::scaled_position(rt.left.tcp->getPoseInRootFrame(), 0.001);
        const Eigen::Matrix4f rightPose = simox::math::scaled_position(rt.right.tcp->getPoseInRootFrame(), 0.001);
 
        rt.virtualPose.block<3, 1>(0, 3) = 0.5 * (leftPose.block<3, 1>(0, 3) + rightPose.block<3, 1>(0, 3));
        rt.virtualPose.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
 
        const Eigen::Matrix4f leftSensorFrame = simox::math::scaled_position(
                rt.left.frameFTSensor->getPoseInRootFrame(), 0.001);
        const Eigen::Matrix4f rightSensorFrame = simox::math::scaled_position(
                    rt.right.frameFTSensor->getPoseInRootFrame(), 0.001);
 
        rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0).transpose() * leftSensorFrame.block<3, 3>(0, 0);
        rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) = rightPose.block<3, 3>(0, 0).transpose() * rightSensorFrame.block<3, 3>(0, 0);
 
        //        ARMARX_INFO << "modified left pose:\n " << leftPose;
        //        ARMARX_INFO << "modified right pose:\n " << rightPose;
    }
 
    std::string NJointBimanualCartesianAdmittanceController::getClassName(const Ice::Current&) const
    {
        return "NJointBimanualCartesianAdmittanceController";
    }
 
    //    void NJointBimanualCartesianAdmittanceController::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 NJointBimanualCartesianAdmittanceController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
    {
 
        const Eigen::Matrix4f currentLeftPose = simox::math::scaled_position(rt.left.tcp->getPoseInRootFrame(), 0.001);
        const Eigen::Matrix4f currentRightPose = simox::math::scaled_position(rt.right.tcp->getPoseInRootFrame(), 0.001);
        const Eigen::Matrix4f currentBoxPose = [&]
        {
            Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
            pose.block<3, 1>(0, 3) = 0.5 * (currentLeftPose.block<3, 1>(0, 3) + currentRightPose.block<3, 1>(0, 3));
            pose.block<3, 3>(0, 0) = currentLeftPose.block<3, 3>(0, 0);
            return pose;
        }();
        //        {
        //            controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
        //            controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
        //            controlInterfaceBuffer.commitWrite();
        //        }
        const double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        ARMARX_ON_SCOPE_EXIT{rt.firstLoop = false;};
 
        rt.ftcalibrationTimer += deltaT;
 
        // -------------------------------------------- config ---------------------------------------------
 
        if (cfgBuf.updateReadBuffer())
        {
            auto& cfg = cfgBuf._getNonConstReadBuffer();
            const Eigen::Vector3f tmpL = rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * cfg.CoMVecLeft;
            const Eigen::Vector3f tmpR = rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * cfg.CoMVecRight;
            cfg.CoMVecLeft = tmpL;
            cfg.CoMVecRight = tmpR;
        }
        if (rt.firstLoop)
        {
            auto& trg = targBuf._getNonConstReadBuffer();
            trg.pose = currentBoxPose;
            trg.vel.setZero();
        }
        auto& dbgOut = debugOutputData.getWriteBuffer();
        const auto& targ = targBuf.getWriteBuffer();
        const auto& cfg = cfgBuf.getReadBuffer();
 
        if (rt.ftcalibrationTimer < cfg.ftCalibrationTime)
        {
            //            rt.ftOffset.block<3, 1>(0, 0) = 0.5 * rt.ftOffset.block<3, 1>(0, 0) + 0.5 * rt.right.forceTorque->force;
            //            rt.ftOffset.block<3, 1>(3, 0) = 0.5 * rt.ftOffset.block<3, 1>(3, 0) + 0.5 * rt.right.forceTorque->torque;
            //            rt.ftOffset.block<3, 1>(6, 0) = 0.5 * rt.ftOffset.block<3, 1>(6, 0) + 0.5 * rt.left.forceTorque->force;
            //            rt.ftOffset.block<3, 1>(9, 0) = 0.5 * rt.ftOffset.block<3, 1>(9, 0) + 0.5 * rt.left.forceTorque->torque;
            //            cfg.KmAdmittance.setZero();
        }
 
        const Eigen::Vector6f KmAdmittance =
            (rt.ftcalibrationTimer < cfg.ftCalibrationTime) ?
            Eigen::Vector6f::Zero() :
            cfg.KmAdmittance;
 
        // -------------------------------------------- target wrench ---------------------------------------------
        const Eigen::Vector12f deltaPoseForWrenchControl = cfg.targetWrench.array() / cfg.KpImpedance.array();
 
        // ------------------------------------------- current tcp pose -------------------------------------------
 
 
        // --------------------------------------------- grasp matrix ---------------------------------------------
        const auto skew = [](auto & vec)
        {
            Eigen::Matrix3f mat = Eigen::MatrixXf::Zero(3, 3);
            mat(1, 2) = -vec(0);
            mat(0, 2) = vec(1);
            mat(0, 1) = -vec(2);
            mat(2, 1) = vec(0);
            mat(2, 0) = -vec(1);
            mat(1, 0) = vec(2);
            return mat;
        };
        const Eigen::Vector3f objCom2TCPLeft{-cfg.boxWidth * 0.5f, 0.f, 0.f};
        const Eigen::Vector3f objCom2TCPRight{+cfg.boxWidth * 0.5f, 0.f, 0.f};
 
        Eigen::Matrix_6_12_f graspMatrix;
        graspMatrix.setZero();
        graspMatrix.block<3, 3>(0, 0) = Eigen::MatrixXf::Identity(3, 3);
        graspMatrix.block<3, 3>(0, 6) = Eigen::MatrixXf::Identity(3, 3);
 
        const Eigen::Vector3f rLeft = rt.virtualPose.block<3, 3>(0, 0) * objCom2TCPLeft;
        const Eigen::Vector3f rRight = rt.virtualPose.block<3, 3>(0, 0) * objCom2TCPRight;
 
        graspMatrix.block<3, 3>(3, 0) = skew(rLeft);
        graspMatrix.block<3, 3>(3, 6) = skew(rRight);
 
        // // projection of grasp matrix
        // Eigen::MatrixXf pinvG = rt.left.IK->computePseudoInverseJacobianMatrix(graspMatrix, 0);
        // Eigen::MatrixXf G_range = pinvG * graspMatrix;
        // Eigen::MatrixXf PG = Eigen::MatrixXf::Identity(12, 12) - G_range;
        float lambda = 1;
        const Eigen::MatrixXf pinvGraspMatrixT = rt.left.IK->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::Vector6f boxCurrentTwist = Eigen::Vector6f::Zero();
 
        // -------------------------------------- get Jacobian matrix and qpos -------------------------------------
        const Eigen::MatrixXf I = Eigen::MatrixXf::Identity(rt.left.targets.size(), rt.left.targets.size());
        // Jacobian matrices
        Eigen::MatrixXf jacobiL = rt.left.IK->getJacobianMatrix(rt.left.tcp, VirtualRobot::IKSolver::CartesianSelection::All);
        Eigen::MatrixXf jacobiR = rt.right.IK->getJacobianMatrix(rt.right.tcp, 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(rt.left.positionSensors.size());
        Eigen::VectorXf leftqvel(rt.left.velocitySensors.size());
        for (size_t i = 0; i < rt.left.velocitySensors.size(); ++i)
        {
            leftqpos(i) = rt.left.positionSensors[i]->position;
            leftqvel(i) = rt.left.velocitySensors[i]->velocity;
        }
 
        Eigen::VectorXf rightqpos(rt.right.positionSensors.size());
        Eigen::VectorXf rightqvel(rt.right.velocitySensors.size());
        for (size_t i = 0; i < rt.right.velocitySensors.size(); ++i)
        {
            rightqpos(i) = rt.right.positionSensors[i]->position;
            rightqvel(i) = rt.right.velocitySensors[i]->velocity;
        }
 
        // -------------------------------------- compute TCP and object velocity -------------------------------------
        const Eigen::Vector6f currentLeftTwist = jacobiL * leftqvel;
        const Eigen::Vector6f currentRightTwist = jacobiR * rightqvel;
 
        Eigen::Vector12f currentTwist;
        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
        const Eigen::Vector3f gravity{0.0, 0.0, -9.8};
        const Eigen::Vector3f localGravityLeft = currentLeftPose.block<3, 3>(0, 0).transpose() * gravity;
        const Eigen::Vector3f localForceVecLeft = cfg.massLeft * localGravityLeft;
        const Eigen::Vector3f localTorqueVecLeft = cfg.CoMVecLeft.cross(localForceVecLeft);
 
        const Eigen::Vector3f localGravityRight = currentRightPose.block<3, 3>(0, 0).transpose() * gravity;
        const Eigen::Vector3f localForceVecRight = cfg.massRight * localGravityRight;
        const Eigen::Vector3f localTorqueVecRight = cfg.CoMVecRight.cross(localForceVecRight);
 
        // mapping of measured wrenches
        Eigen::Vector12f wrenchesMeasured;
        wrenchesMeasured << rt.right.forceTorque->force - cfg.forceOffsetLeft,
                         rt.right.forceTorque->torque - cfg.torqueOffsetLeft,
                         rt.left.forceTorque->force - cfg.forceOffsetRight,
                         rt.left.forceTorque->torque - cfg.torqueOffsetRight;
        for (size_t i = 0; i < 12; i++)
        {
            wrenchesMeasured(i) = (1 - cfg.filterCoeff) * wrenchesMeasured(i) + cfg.filterCoeff * rt.filteredOldValue(i);
        }
        rt.filteredOldValue = wrenchesMeasured;
        wrenchesMeasured.block<3, 1>(0, 0) = rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(0, 0) - localForceVecLeft;
        wrenchesMeasured.block<3, 1>(3, 0) = rt.left.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(3, 0) - localTorqueVecLeft;
        wrenchesMeasured.block<3, 1>(6, 0) = rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(6, 0) - localForceVecRight;
        wrenchesMeasured.block<3, 1>(9, 0) = rt.right.sensorFrame2TcpFrame.block<3, 3>(0, 0) * wrenchesMeasured.block<3, 1>(9, 0) - localTorqueVecRight;
        //        if (wrenchesMeasured.norm() < cfg->forceThreshold)
        //        {
        //            wrenchesMeasured.setZero();
        //        }
 
        Eigen::Vector12f wrenchesMeasuredInRoot;
        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::Vector6f objFTValue = graspMatrix * wrenchesMeasuredInRoot;
        for (size_t i = 0; i < 6; i++)
        {
            if (fabs(objFTValue(i)) < cfg.forceThreshold(i))
            {
                objFTValue(i) = 0;
            }
            else
            {
                objFTValue(i) -= cfg.forceThreshold(i) * objFTValue(i) / fabs(objFTValue(i));
            }
        }
 
        // --------------------------------------------- get MP target ---------------------------------------------
        const Eigen::Matrix4f boxPose = targ.pose;
        const Eigen::Vector6f boxTwist = targ.vel;
        // --------------------------------------------- obj admittance control ---------------------------------------------
        // admittance
        Eigen::Vector6f objPoseError;
        objPoseError.head(3) = rt.virtualPose.block<3, 1>(0, 3) - boxPose.block<3, 1>(0, 3);
        const Eigen::Matrix3f objDiffMat = rt.virtualPose.block<3, 3>(0, 0) * boxPose.block<3, 3>(0, 0).transpose();
        objPoseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
 
 
        Eigen::Vector6f objAcc = Eigen::Vector6f::Zero();
        Eigen::Vector6f objVel = Eigen::Vector6f::Zero();
        for (size_t i = 0; i < 6; i++)
        {
            objAcc(i) = KmAdmittance(i) * objFTValue(i)
                        - cfg.KpAdmittance(i) * objPoseError(i)
                        - cfg.KdAdmittance(i) * rt.virtualVel(i);
        }
        objVel = rt.virtualVel + 0.5 * deltaT * (objAcc + rt.virtualAcc);
        const Eigen::Vector6f deltaObjPose = 0.5 * deltaT * (objVel + rt.virtualVel);
        rt.virtualAcc = objAcc;
        rt.virtualVel = objVel;
        rt.virtualPose.block<3, 1>(0, 3) += deltaObjPose.head(3);
        rt.virtualPose.block<3, 3>(0, 0) = VirtualRobot::MathTools::rpy2eigen3f(
                                               deltaObjPose(3),
                                               deltaObjPose(4),
                                               deltaObjPose(5)) * rt.virtualPose.block<3, 3>(0, 0);
 
        // --------------------------------------------- convert to tcp pose ---------------------------------------------
        Eigen::Matrix4f tcpTargetPoseLeft = rt.virtualPose;
        Eigen::Matrix4f tcpTargetPoseRight = rt.virtualPose;
        tcpTargetPoseLeft.block<3, 1>(0, 3) += rt.virtualPose.block<3, 3>(0, 0) * (objCom2TCPLeft - deltaPoseForWrenchControl.block<3, 1>(0, 0));
        tcpTargetPoseRight.block<3, 1>(0, 3) += rt.virtualPose.block<3, 3>(0, 0) * (objCom2TCPRight - deltaPoseForWrenchControl.block<3, 1>(6, 0));
 
        // --------------------------------------------- Impedance control ---------------------------------------------
        Eigen::Vector12f poseError;
        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::Vector12f forceImpedance;
        for (size_t i = 0; i < 12; i++)
        {
            forceImpedance(i) = cfg.KpImpedance(i) * poseError(i) - cfg.KdImpedance(i) * currentTwist(i);
            //            forceImpedance(i + 6) = KpImpedance(i) * poseError(i + 6) - KdImpedance(i) * currentTwist(i + 6);
        }
 
        // --------------------------------------------- Nullspace control ---------------------------------------------
        const Eigen::VectorXf leftNullspaceTorque  = cfg.knull * (cfg.desiredJointValuesLeft  - leftqpos) - cfg.dnull * leftqvel;
        const Eigen::VectorXf rightNullspaceTorque = cfg.knull * (cfg.desiredJointValuesRight - rightqpos) - cfg.dnull * rightqvel;
 
        // --------------------------------------------- Set Torque Control Command ---------------------------------------------
        //        float lambda = 1;
 
        // torque limit
        const auto setTargets = [&](auto & rtarm, const auto & jacobi, const auto & nullspaceTorque, int forceImpOffset)
        {
            const Eigen::MatrixXf jtpinv = rtarm.IK->computePseudoInverseJacobianMatrix(jacobi.transpose(), lambda);
            const Eigen::VectorXf desiredJointTorques = jacobi.transpose() * forceImpedance.block<6, 1>(forceImpOffset, 0) +
                    (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
            for (size_t i = 0; i < rtarm.targets.size(); ++i)
            {
                float desiredTorque =   desiredJointTorques(i);
                if (isnan(desiredTorque))
                {
                    desiredTorque = 0;
                }
                desiredTorque = (desiredTorque >  cfg.torqueLimit) ? cfg.torqueLimit : desiredTorque;
                desiredTorque = (desiredTorque < -cfg.torqueLimit) ? -cfg.torqueLimit : desiredTorque;
                dbgOut.desired_torques[rtarm.jointNames[i]] = desiredJointTorques(i);
                rtarm.targets.at(i)->torque = desiredTorque;
            }
        };
        setTargets(rt.left,  jacobiL, leftNullspaceTorque, 0);
        setTargets(rt.right, jacobiR, rightNullspaceTorque, 6);
        {
            const Eigen::MatrixXf jtpinvL = rt.left.IK->computePseudoInverseJacobianMatrix(jacobiL.transpose(), lambda);
            const Eigen::VectorXf leftJointDesiredTorques = jacobiL.transpose() * forceImpedance.head(6) + (I - jacobiL.transpose() * jtpinvL) * leftNullspaceTorque;
            for (size_t i = 0; i < rt.left.targets.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;
                dbgOut.desired_torques[rt.left.jointNames[i]] = leftJointDesiredTorques(i);
                rt.left.targets.at(i)->torque = desiredTorque;
            }
        }
 
        {
            const Eigen::MatrixXf jtpinvR = rt.right.IK->computePseudoInverseJacobianMatrix(jacobiR.transpose(), lambda);
            const Eigen::VectorXf rightJointDesiredTorques = jacobiR.transpose() * forceImpedance.tail(6) + (I - jacobiR.transpose() * jtpinvR) * rightNullspaceTorque;
            for (size_t i = 0; i < rt.right.targets.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;
                dbgOut.desired_torques[rt.right.jointNames[i]] = rightJointDesiredTorques(i);
                rt.right.targets.at(i)->torque = desiredTorque;
            }
        }
 
        // --------------------------------------------- debug output ---------------------------------------------
        dbgOut.currentBoxPose = currentBoxPose;
        dbgOut.forceImpedance = forceImpedance;
        dbgOut.poseError = poseError;
        //        dbgOut.wrenchesConstrained = wrenchesConstrained;
        dbgOut.wrenchesMeasuredInRoot = wrenchesMeasuredInRoot;
        //        dbgOut.wrenchDMP = wrenchDMP;
        //        dbgOut.computedBoxWrench = computedBoxWrench;
 
        dbgOut.virtualPose_x = rt.virtualPose(0, 3);
        dbgOut.virtualPose_y = rt.virtualPose(1, 3);
        dbgOut.virtualPose_z = rt.virtualPose(2, 3);
 
        dbgOut.objPose_x = boxCurrentPose(0, 3);
        dbgOut.objPose_y = boxCurrentPose(1, 3);
        dbgOut.objPose_z = boxCurrentPose(2, 3);
 
        dbgOut.objForce_x = objFTValue(0);
        dbgOut.objForce_y = objFTValue(1);
        dbgOut.objForce_z = objFTValue(2);
        dbgOut.objTorque_x = objFTValue(3);
        dbgOut.objTorque_y = objFTValue(4);
        dbgOut.objTorque_z = objFTValue(5);
 
        dbgOut.objVel_x = objVel(0);
        dbgOut.objVel_y = objVel(1);
        dbgOut.objVel_z = objVel(2);
        dbgOut.objVel_rx = objVel(3);
        dbgOut.objVel_ry = objVel(4);
        dbgOut.objVel_rz = objVel(5);
 
        dbgOut.deltaPose_x = deltaObjPose(0);
        dbgOut.deltaPose_y = deltaObjPose(1);
        dbgOut.deltaPose_z = deltaObjPose(2);
        dbgOut.deltaPose_rx = deltaObjPose(3);
        dbgOut.deltaPose_ry = deltaObjPose(4);
        dbgOut.deltaPose_rz = deltaObjPose(5);
 
        dbgOut.modifiedPoseRight_x = tcpTargetPoseRight(0, 3);
        dbgOut.modifiedPoseRight_y = tcpTargetPoseRight(1, 3);
        dbgOut.modifiedPoseRight_z = tcpTargetPoseRight(2, 3);
 
        dbgOut.currentPoseLeft_x = currentLeftPose(0, 3);
        dbgOut.currentPoseLeft_y = currentLeftPose(1, 3);
        dbgOut.currentPoseLeft_z = currentLeftPose(2, 3);
 
 
 
        dbgOut.modifiedPoseLeft_x = tcpTargetPoseLeft(0, 3);
        dbgOut.modifiedPoseLeft_y = tcpTargetPoseLeft(1, 3);
        dbgOut.modifiedPoseLeft_z = tcpTargetPoseLeft(2, 3);
 
        dbgOut.currentPoseRight_x = currentRightPose(0, 3);
        dbgOut.currentPoseRight_y = currentRightPose(1, 3);
        dbgOut.currentPoseRight_z = currentRightPose(2, 3);
 
 
        dbgOut.dmpBoxPose_x = boxPose(0, 3);
        dbgOut.dmpBoxPose_y = boxPose(1, 3);
        dbgOut.dmpBoxPose_z = boxPose(2, 3);
 
        dbgOut.dmpTwist_x = boxTwist(0);
        dbgOut.dmpTwist_y = boxTwist(1);
        dbgOut.dmpTwist_z = boxTwist(2);
        dbgOut.rx = rRight(0);
        dbgOut.ry = rRight(1);
        dbgOut.rz = rRight(2);
 
        //        dbgOut.modifiedTwist_lx = twistDMP(0);
        //        dbgOut.modifiedTwist_ly = twistDMP(1);
        //        dbgOut.modifiedTwist_lz = twistDMP(2);
        //        dbgOut.modifiedTwist_rx = twistDMP(6);
        //        dbgOut.modifiedTwist_ry = twistDMP(7);
        //        dbgOut.modifiedTwist_rz = twistDMP(8);
 
        //        dbgOut.forcePID = forcePIDInRootForDebug;
 
        debugOutputData.commitWrite();
 
    }
 
    void NJointBimanualCartesianAdmittanceController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
    {
        std::lock_guard guard{debugOutputDataReadMutex};
        const auto& buf = debugOutputData.getUpToDateReadBuffer();
        StringVariantBaseMap datafields;
 
        for (const auto& [name, val] : buf.desired_torques)
        {
            datafields[name] = new Variant(val);
        }
 
        const auto& reportElements = [&](const auto & vec, const std::string & pre)
        {
            for (int i = 0; i < vec.rows(); ++i)
            {
                datafields[pre + std::to_string(i)] = new Variant(vec(i));
            }
        };
        reportElements(buf.forceImpedance, "forceImpedance_");
        reportElements(buf.forcePID, "forcePID_");
        reportElements(buf.poseError, "poseError_");
        reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
        reportElements(buf.wrenchesMeasuredInRoot, "wrenchesMeasuredInRoot_");
        reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
        reportElements(buf.wrenchesConstrained, "wrenchesConstrained_");
 
        datafields["virtualPose_x"] = new Variant(buf.virtualPose_x);
        datafields["virtualPose_y"] = new Variant(buf.virtualPose_y);
        datafields["virtualPose_z"] = new Variant(buf.virtualPose_z);
 
        datafields["objPose_x"] = new Variant(buf.objPose_x);
        datafields["objPose_y"] = new Variant(buf.objPose_y);
        datafields["objPose_z"] = new Variant(buf.objPose_z);
 
        datafields["objForce_x"] = new Variant(buf.objForce_x);
        datafields["objForce_y"] = new Variant(buf.objForce_y);
        datafields["objForce_z"] = new Variant(buf.objForce_z);
        datafields["objTorque_x"] = new Variant(buf.objTorque_x);
        datafields["objTorque_y"] = new Variant(buf.objTorque_y);
        datafields["objTorque_z"] = new Variant(buf.objTorque_z);
 
        datafields["objVel_x"] = new Variant(buf.objVel_x);
        datafields["objVel_y"] = new Variant(buf.objVel_y);
        datafields["objVel_z"] = new Variant(buf.objVel_z);
        datafields["objVel_rx"] = new Variant(buf.objVel_rx);
        datafields["objVel_ry"] = new Variant(buf.objVel_ry);
        datafields["objVel_rz"] = new Variant(buf.objVel_rz);
 
        datafields["deltaPose_x"] = new Variant(buf.deltaPose_x);
        datafields["deltaPose_y"] = new Variant(buf.deltaPose_y);
        datafields["deltaPose_z"] = new Variant(buf.deltaPose_z);
        datafields["deltaPose_rx"] = new Variant(buf.deltaPose_rx);
        datafields["deltaPose_ry"] = new Variant(buf.deltaPose_ry);
        datafields["deltaPose_rz"] = new Variant(buf.deltaPose_rz);
 
        datafields["modifiedPoseRight_x"] = new Variant(buf.modifiedPoseRight_x);
        datafields["modifiedPoseRight_y"] = new Variant(buf.modifiedPoseRight_y);
        datafields["modifiedPoseRight_z"] = new Variant(buf.modifiedPoseRight_z);
        datafields["currentPoseLeft_x"] = new Variant(buf.currentPoseLeft_x);
        datafields["currentPoseLeft_y"] = new Variant(buf.currentPoseLeft_y);
        datafields["currentPoseLeft_z"] = new Variant(buf.currentPoseLeft_z);
 
 
        datafields["modifiedPoseLeft_x"] = new Variant(buf.modifiedPoseLeft_x);
        datafields["modifiedPoseLeft_y"] = new Variant(buf.modifiedPoseLeft_y);
        datafields["modifiedPoseLeft_z"] = new Variant(buf.modifiedPoseLeft_z);
 
        datafields["currentPoseRight_x"] = new Variant(buf.currentPoseRight_x);
        datafields["currentPoseRight_y"] = new Variant(buf.currentPoseRight_y);
        datafields["currentPoseRight_z"] = new Variant(buf.currentPoseRight_z);
        datafields["dmpBoxPose_x"] = new Variant(buf.dmpBoxPose_x);
        datafields["dmpBoxPose_y"] = new Variant(buf.dmpBoxPose_y);
        datafields["dmpBoxPose_z"] = new Variant(buf.dmpBoxPose_z);
        datafields["dmpTwist_x"] = new Variant(buf.dmpTwist_x);
        datafields["dmpTwist_y"] = new Variant(buf.dmpTwist_y);
        datafields["dmpTwist_z"] = new Variant(buf.dmpTwist_z);
 
        datafields["modifiedTwist_lx"] = new Variant(buf.modifiedTwist_lx);
        datafields["modifiedTwist_ly"] = new Variant(buf.modifiedTwist_ly);
        datafields["modifiedTwist_lz"] = new Variant(buf.modifiedTwist_lz);
        datafields["modifiedTwist_rx"] = new Variant(buf.modifiedTwist_rx);
        datafields["modifiedTwist_ry"] = new Variant(buf.modifiedTwist_ry);
        datafields["modifiedTwist_rz"] = new Variant(buf.modifiedTwist_rz);
 
        datafields["rx"] = new Variant(buf.rx);
        datafields["ry"] = new Variant(buf.ry);
        datafields["rz"] = new Variant(buf.rz);
 
 
        debugObs->setDebugChannel("BimanualForceController", datafields);
    }
 
    Eigen::Matrix4f NJointBimanualCartesianAdmittanceController::getBoxPose(const Ice::Current&) const
    {
        std::lock_guard guard{debugOutputDataReadMutex};
        return debugOutputData.getUpToDateReadBuffer().currentBoxPose;
    }
 
    void NJointBimanualCartesianAdmittanceController::setBoxPose(const Eigen::Matrix4f& pose, const Ice::Current&)
    {
        ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
        std::lock_guard guard{targBufWriteMutex};
        targBuf.getWriteBuffer().pose = pose;
        targBuf.commitWrite();
    }
 
    void NJointBimanualCartesianAdmittanceController::setBoxWidth(float w, const Ice::Current&)
    {
        ARMARX_CHECK_GREATER_EQUAL(w, 0);
        std::lock_guard{cfgBufWriteMutex};
        cfgBuf.getWriteBuffer().boxWidth = w;
        cfgBuf.commitWrite();
    }
    void NJointBimanualCartesianAdmittanceController::setBoxVelocity(
        const Eigen::Vector3f& velXYZ,
        const Eigen::Vector3f& velRPY,
        const Ice::Current&)
    {
        ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velXYZ));
        ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velRPY));
        std::lock_guard guard{targBufWriteMutex};
        targBuf.getWriteBuffer().vel.head<3>() = velXYZ;
        targBuf.getWriteBuffer().vel.tail<3>() = velRPY;
        targBuf.commitWrite();
 
    }
    void NJointBimanualCartesianAdmittanceController::setBoxPoseAndVelocity(
        const Eigen::Matrix4f& pose,
        const Eigen::Vector3f& velXYZ,
        const Eigen::Vector3f& velRPY, const Ice::Current&)
    {
        ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
        ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velXYZ));
        ARMARX_CHECK_EXPRESSION(simox::math::isfinite(velRPY));
        std::lock_guard guard{targBufWriteMutex};
        targBuf.getWriteBuffer().pose = pose;
        targBuf.getWriteBuffer().vel.head<3>() = velXYZ;
        targBuf.getWriteBuffer().vel.tail<3>() = velRPY;
        targBuf.commitWrite();
    }
    void NJointBimanualCartesianAdmittanceController::moveBoxPose(const Eigen::Matrix4f& pose, const Ice::Current&)
    {
        ARMARX_CHECK_EXPRESSION(simox::math::is_homogeneous_transform(pose));
        std::lock_guard guard{targBufWriteMutex};
        const Eigen::Matrix4f tmp = pose * targBuf.getWriteBuffer().pose;
        targBuf.getWriteBuffer().pose = tmp;
        targBuf.commitWrite();
    }
    void NJointBimanualCartesianAdmittanceController::moveBoxPosition(const Eigen::Vector3f& pos, const Ice::Current&)
    {
        ARMARX_CHECK_EXPRESSION(simox::math::isfinite(pos));
        std::lock_guard guard{targBufWriteMutex};
        targBuf.getWriteBuffer().pose.topRightCorner<3, 1>() += pos;
        targBuf.commitWrite();
    }
 
    void NJointBimanualCartesianAdmittanceController::setConfig(const NJointBimanualCartesianAdmittanceControllerConfigPtr& ptr, const Ice::Current&)
    {
        ARMARX_CHECK_NOT_NULL(ptr);
        ARMARX_CHECK_EQUAL(ptr->nullspace.desiredJointValuesLeft.size(), rt.left.targets.size());
        ARMARX_CHECK_EQUAL(ptr->nullspace.desiredJointValuesRight.size(), rt.right.targets.size());
        std::lock_guard{cfgBufWriteMutex};
        updateConfig(*ptr);
        cfgBuf.commitWrite();
    }
    void NJointBimanualCartesianAdmittanceController::setDesiredJointValuesLeft(const Ice::FloatSeq& vals, const Ice::Current&)
    {
        std::lock_guard{cfgBufWriteMutex};
        updateDesiredJointValuesLeft(vals);
        cfgBuf.commitWrite();
    }
    void NJointBimanualCartesianAdmittanceController::setDesiredJointValuesRight(const Ice::FloatSeq& vals, const Ice::Current&)
    {
        std::lock_guard{cfgBufWriteMutex};
        updateDesiredJointValuesRight(vals);
        cfgBuf.commitWrite();
    }
    void NJointBimanualCartesianAdmittanceController::setNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace, const Ice::Current&)
    {
        std::lock_guard{cfgBufWriteMutex};
        updateNullspaceConfig(nullspace);
        cfgBuf.commitWrite();
    }
    void NJointBimanualCartesianAdmittanceController::setAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance& admittanceObject, const Ice::Current&)
    {
        std::lock_guard{cfgBufWriteMutex};
        updateAdmittanceConfig(admittanceObject);
        cfgBuf.commitWrite();
    }
    void NJointBimanualCartesianAdmittanceController::setForceConfig(const detail::NJBmanCartAdmCtrl::Force& left, const detail::NJBmanCartAdmCtrl::Force& right, const Ice::Current&)
    {
        std::lock_guard{cfgBufWriteMutex};
        updateForceConfig(left, right);
        cfgBuf.commitWrite();
    }
    void NJointBimanualCartesianAdmittanceController::setImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& left, const detail::NJBmanCartAdmCtrl::Impedance& right, const Ice::Current&)
    {
        std::lock_guard{cfgBufWriteMutex};
        updateImpedanceConfig(left, right);
        cfgBuf.commitWrite();
    }
 
    void NJointBimanualCartesianAdmittanceController::updateConfig(const NJointBimanualCartesianAdmittanceControllerConfig& cfg)
    {
        std::lock_guard{cfgBufWriteMutex};
        auto& buf = cfgBuf.getWriteBuffer();
        updateNullspaceConfig(cfg.nullspace);
        updateAdmittanceConfig(cfg.admittanceObject);
        updateForceConfig(cfg.forceLeft, cfg.forceRight);
        updateImpedanceConfig(cfg.impedanceLeft, cfg.impedanceRight);
        buf.torqueLimit = cfg.torqueLimit;
        buf.filterCoeff = cfg.filterCoeff;
        buf.ftCalibrationTime = cfg.ftCalibrationTime;
        buf.boxWidth = cfg.box.width;
    }
    void NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesLeft(const Ice::FloatSeq& vals)
    {
        std::lock_guard{cfgBufWriteMutex};
        auto& buf = cfgBuf.getWriteBuffer();
        ARMARX_CHECK_EQUAL(vals.size(), rt.left.targets.size());
        buf.desiredJointValuesLeft = Eigen::Map<const Eigen::VectorXf>(
                                         vals.data(), vals.size());
    }
    void NJointBimanualCartesianAdmittanceController::updateDesiredJointValuesRight(const Ice::FloatSeq& vals)
    {
        std::lock_guard{cfgBufWriteMutex};
        auto& buf = cfgBuf.getWriteBuffer();
        ARMARX_CHECK_EQUAL(vals.size(), rt.right.targets.size());
        buf.desiredJointValuesRight = Eigen::Map<const Eigen::VectorXf>(
                                          vals.data(), vals.size());
    }
    void NJointBimanualCartesianAdmittanceController::updateNullspaceConfig(const detail::NJBmanCartAdmCtrl::Nullspace& nullspace)
    {
        std::lock_guard{cfgBufWriteMutex};
        auto& cfg = cfgBuf.getWriteBuffer();
        updateDesiredJointValuesLeft(nullspace.desiredJointValuesLeft);
        updateDesiredJointValuesRight(nullspace.desiredJointValuesRight);
        cfg.knull = nullspace.k;
        cfg.dnull = nullspace.d;
    }
    void NJointBimanualCartesianAdmittanceController::updateAdmittanceConfig(const detail::NJBmanCartAdmCtrl::Admittance admittanceObject)
    {
        std::lock_guard{cfgBufWriteMutex};
        auto& cfg = cfgBuf.getWriteBuffer();
        cfg.KmAdmittance.block<3, 1>(0, 0) = admittanceObject.KmXYZ;
        cfg.KmAdmittance.block<3, 1>(3, 0) = admittanceObject.KmRPY;
 
        cfg.KpAdmittance.block<3, 1>(0, 0) = admittanceObject.KpXYZ;
        cfg.KpAdmittance.block<3, 1>(3, 0) = admittanceObject.KpRPY;
 
        cfg.KdAdmittance.block<3, 1>(0, 0) = admittanceObject.KdXYZ;
        cfg.KdAdmittance.block<3, 1>(3, 0) = admittanceObject.KdRPY;
    }
    void NJointBimanualCartesianAdmittanceController::updateForceConfig(const detail::NJBmanCartAdmCtrl::Force& forceLeft, const detail::NJBmanCartAdmCtrl::Force& forceRight)
    {
        std::lock_guard{cfgBufWriteMutex};
        auto& cfg = cfgBuf.getWriteBuffer();
        //left
        cfg.massLeft = forceLeft.mass;
        cfg.CoMVecLeft = forceLeft.com;
        cfg.forceOffsetLeft = forceLeft.offsetForce;
        cfg.torqueOffsetLeft = forceLeft.offsetTorque;
        cfg.targetWrench.block<3, 1>(0, 0) = forceLeft.wrenchXYZ;
        cfg.targetWrench.block<3, 1>(3, 0) = forceLeft.wrenchRPY;
        cfg.forceThreshold.block<3, 1>(0, 0) = forceLeft.forceThreshold;
        //right
        cfg.massRight = forceRight.mass;
        cfg.CoMVecRight = forceRight.com;
        cfg.forceOffsetRight = forceRight.offsetForce;
        cfg.torqueOffsetRight = forceRight.offsetTorque;
        cfg.targetWrench.block<3, 1>(6, 0) = forceRight.wrenchXYZ;
        cfg.targetWrench.block<3, 1>(9, 0) = forceRight.wrenchRPY;
        cfg.forceThreshold.block<3, 1>(3, 0) = forceRight.forceThreshold;
    }
    void NJointBimanualCartesianAdmittanceController::updateImpedanceConfig(const detail::NJBmanCartAdmCtrl::Impedance& impedanceLeft,
            const detail::NJBmanCartAdmCtrl::Impedance& impedanceRight)
    {
        std::lock_guard{cfgBufWriteMutex};
        auto& cfg = cfgBuf.getWriteBuffer();
        cfg.KpImpedance.block<3, 1>(0, 0) = impedanceLeft.KpXYZ;
        cfg.KpImpedance.block<3, 1>(3, 0) = impedanceLeft.KpRPY;
        cfg.KpImpedance.block<3, 1>(6, 0) = impedanceRight.KpXYZ;
        cfg.KpImpedance.block<3, 1>(9, 0) = impedanceRight.KpRPY;
 
        cfg.KdImpedance.block<3, 1>(0, 0) = impedanceLeft.KdXYZ;
        cfg.KdImpedance.block<3, 1>(3, 0) = impedanceLeft.KdRPY;
        cfg.KdImpedance.block<3, 1>(6, 0) = impedanceRight.KdXYZ;
        cfg.KdImpedance.block<3, 1>(9, 0) = impedanceRight.KdRPY;
    }
}