NJointBimanualObjLevelVelController.cpp

Go to the documentation of this file.

#include "NJointBimanualObjLevelVelController.h"
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
// Simox
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Nodes/RobotNode.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/CartesianVelocityController.h>
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
 
namespace armarx::control::deprecated_njoint_mp_controller::bimanual
{
    NJointControllerRegistration<NJointBimanualObjLevelVelController>
        registrationControllerNJointBimanualObjLevelVelController(
            "NJointBimanualObjLevelVelController");
 
    NJointBimanualObjLevelVelController::NJointBimanualObjLevelVelController(
        const RobotUnitPtr& robUnit,
        const armarx::NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        ARMARX_INFO << "Preparing ... bimanual ";
        ARMARX_INFO << "I am here";
 
        useSynchronizedRtRobot();
        cfg = NJointBimanualObjLevelVelControllerConfigPtr::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::Velocity1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            leftTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No 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::Velocity1DoF);
            const SensorValueBase* sv = useSensorValue(jointName);
            rightTargets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
 
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
 
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No velocitySensor available for " << jointName;
            }
            if (!positionSensor)
            {
                ARMARX_WARNING << "No positionSensor available for " << jointName;
            }
 
            rightVelocitySensors.push_back(velocitySensor);
            rightPositionSensors.push_back(positionSensor);
        };
 
        leftIK.reset(new VirtualRobot::DifferentialIK(
            leftRNS, leftRNS->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
        rightIK.reset(new VirtualRobot::DifferentialIK(rightRNS,
                                                       rightRNS->getRobot()->getRootNode(),
                                                       VirtualRobot::JacobiProvider::eSVDDamped));
 
 
        leftTCPController.reset(new CartesianVelocityController(leftRNS, leftRNS->getTCP()));
        rightTCPController.reset(new CartesianVelocityController(rightRNS, rightRNS->getTCP()));
 
        double phaseL = 10;
        double phaseK = 10;
        double phaseDist0 = 50;
        double phaseDist1 = 10;
        double phaseKpPos = 1;
        double phaseKpOri = 0.1;
        double posToOriRatio = 10;
        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.Dori = 0;
        taskSpaceDMPConfig.phaseStopParas.Dpos = 0;
        taskSpaceDMPConfig.phaseStopParas.goDist = phaseDist0;
        taskSpaceDMPConfig.phaseStopParas.backDist = phaseDist1;
        taskSpaceDMPConfig.phaseStopParas.Kpos = phaseKpPos;
        taskSpaceDMPConfig.phaseStopParas.Kori = phaseKpOri;
        taskSpaceDMPConfig.phaseStopParas.mm2radi = posToOriRatio;
        taskSpaceDMPConfig.phaseStopParas.maxValue = phaseL;
        taskSpaceDMPConfig.phaseStopParas.slop = phaseK;
 
 
        objectDMP.reset(new tsvmp::TaskSpaceDMPController("boxDMP", taskSpaceDMPConfig, false));
        ARMARX_IMPORTANT << "dmp finished";
 
        tcpLeft = leftRNS->getTCP();
        tcpRight = rightRNS->getTCP();
 
        //initialize control parameters
        KpImpedance.setZero(cfg->KpImpedance.size());
        ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 6);
 
        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(), 6);
 
        for (int i = 0; i < KdImpedance.size(); ++i)
        {
            KdImpedance(i) = cfg->KdImpedance.at(i);
        }
 
        Inferface2rtData initInt2rtData;
        initInt2rtData.KpImpedance = KpImpedance;
        initInt2rtData.KdImpedance = KdImpedance;
        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);
        }
 
        virtualPose = Eigen::Matrix4f::Identity();
        ARMARX_INFO << "got controller params";
 
        rt2ControlData initSensorData;
        initSensorData.deltaT = 0;
        initSensorData.currentTime = 0;
        initSensorData.currentPose = leftRNS->getTCP()->getPoseInRootFrame();
        initSensorData.currentTwist.setZero();
        rt2ControlBuffer.reinitAllBuffers(initSensorData);
 
 
        ControlInterfaceData initInterfaceData;
        initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
        initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
        initInterfaceData.currentObjPose = leftRNS->getTCP()->getPoseInRootFrame();
        initInterfaceData.currentObjVel.setZero();
        controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
 
 
        leftInitialPose = tcpLeft->getPoseInRootFrame();
        rightInitialPose = tcpRight->getPoseInRootFrame();
        leftInitialPose.block<3, 1>(0, 3) = leftInitialPose.block<3, 1>(0, 3);
        rightInitialPose.block<3, 1>(0, 3) = rightInitialPose.block<3, 1>(0, 3);
 
        // TODO the following is only predefined for balance ball
        fixedLeftRightRotOffset = Eigen::Matrix3f::Identity();
 
        Eigen::Matrix4f rightLeveledRotation =
            VirtualRobot::MathTools::quat2eigen4f(0.5, -0.5, -0.5, -0.5);
        Eigen::Matrix4f leftLeveledRotation =
            VirtualRobot::MathTools::quat2eigen4f(0.5, 0.5, 0.5, -0.5);
        fixedLeftRightRotOffset = leftLeveledRotation.block<3, 3>(0, 0).transpose() *
                                  rightLeveledRotation.block<3, 3>(0, 0);
 
 
        boxInitialPose = leftInitialPose;
        boxInitialPose(0, 3) = (leftInitialPose(0, 3) + rightInitialPose(0, 3)) / 2;
        boxInitialPose(1, 3) = (leftInitialPose(1, 3) + rightInitialPose(1, 3)) / 2;
        boxInitialPose(2, 3) = (leftInitialPose(2, 3) + rightInitialPose(2, 3)) / 2;
 
        NJointBimanualObjLevelVelControlData initData;
        initData.boxPose = boxInitialPose;
        reinitTripleBuffer(initData);
 
        dmpGoal = boxInitialPose;
 
        firstLoop = true;
        ARMARX_INFO << "left initial pose: \n"
                    << leftInitialPose << "\n right initial pose: \n"
                    << rightInitialPose;
        dmpStarted = false;
        objCom2TCPLeftInObjFrame.setZero();
        objCom2TCPRightInObjFrame.setZero();
    }
 
    void
    NJointBimanualObjLevelVelController::setMPWeights(const DoubleSeqSeq& weights,
                                                      const Ice::Current&)
    {
        objectDMP->setWeights(weights);
    }
 
    DoubleSeqSeq
    NJointBimanualObjLevelVelController::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
    NJointBimanualObjLevelVelController::setMPRotWeights(const DoubleSeqSeq& weights,
                                                         const Ice::Current&)
    {
        objectDMP->setRotWeights(weights);
    }
 
    DoubleSeqSeq
    NJointBimanualObjLevelVelController::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
    NJointBimanualObjLevelVelController::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);
        rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3);
        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);
 
        NJointBimanualObjLevelVelControlData initData;
        initData.boxPose = boxInitPose;
        reinitTripleBuffer(initData);
    }
 
    std::string
    NJointBimanualObjLevelVelController::getClassName(const Ice::Current&) const
    {
        return "NJointBimanualObjLevelVelController";
    }
 
    void
    NJointBimanualObjLevelVelController::controllerRun()
    {
        if (!rt2ControlBuffer.updateReadBuffer() || !dmpStarted)
        {
            return;
        }
 
        double deltaT = rt2ControlBuffer.getReadBuffer().deltaT;
        Eigen::Matrix4f currentPose = rt2ControlBuffer.getReadBuffer().currentPose;
        Eigen::VectorXf currentTwist = rt2ControlBuffer.getReadBuffer().currentTwist;
 
        if (objectDMP->canVal < 0)
        {
            finished = true;
            dmpStarted = false;
            LockGuardType guard{controlDataMutex};
            getWriterControlStruct().boxPose = dmpGoal;
            writeControlStruct();
        }
        else
        {
            objectDMP->flow(deltaT, currentPose, currentTwist);
            //            VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(objectDMP->getTargetPoseMat());
            LockGuardType guard{controlDataMutex};
            getWriterControlStruct().boxPose = objectDMP->getTargetPoseMat();
            writeControlStruct();
        }
    }
 
    Eigen::VectorXf
    NJointBimanualObjLevelVelController::calcIK(VirtualRobot::DifferentialIKPtr ik,
                                                const Eigen::MatrixXf& jacobi,
                                                const Eigen::VectorXf& cartesianVel,
                                                const Eigen::VectorXf& nullspaceVel)
    {
        Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
 
        Eigen::MatrixXf nullspace = lu_decomp.kernel();
        Eigen::VectorXf nsv = Eigen::VectorXf::Zero(nullspace.rows());
        for (int i = 0; i < nullspace.cols(); i++)
        {
            float squaredNorm = nullspace.col(i).squaredNorm();
            // Prevent division by zero
            if (squaredNorm > 1.0e-32f)
            {
                nsv += nullspace.col(i) * nullspace.col(i).dot(nullspaceVel) /
                       nullspace.col(i).squaredNorm();
            }
        }
 
        Eigen::MatrixXf inv = ik->computePseudoInverseJacobianMatrix(
            jacobi, ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
        Eigen::VectorXf jointVel = inv * cartesianVel;
        return jointVel;
    }
 
    void
    NJointBimanualObjLevelVelController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                               const IceUtil::Time& timeSinceLastIteration)
    {
        Eigen::Matrix4f currentLeftPose = tcpLeft->getPoseInRootFrame();
        Eigen::Matrix4f currentRightPose = tcpRight->getPoseInRootFrame();
 
        double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        if (firstLoop)
        {
            Eigen::Matrix4f leftPose = tcpLeft->getPoseInRootFrame();
            Eigen::Matrix4f rightPose = tcpRight->getPoseInRootFrame();
 
            leftPose.block<3, 1>(0, 3) = leftPose.block<3, 1>(0, 3);
            rightPose.block<3, 1>(0, 3) = rightPose.block<3, 1>(0, 3);
 
            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);
 
            objCom2TCPLeftInObjFrame =
                virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPLeftInGlobal;
            objCom2TCPRightInObjFrame =
                virtualPose.block<3, 3>(0, 0).transpose() * objCom2TCPRightInGlobal;
            firstLoop = false;
        }
 
        // --------------------------------------------- get control parameters ---------------------------------------
        KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
        KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
 
        // --------------------------------------------- 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);
        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);
 
        float lambda = 1;
        Eigen::MatrixXf pinvGraspMatrixT =
            leftIK->computePseudoInverseJacobianMatrix(graspMatrix.transpose(), lambda);
 
        // ---------------------------------------------- object pose ----------------------------------------------
        Eigen::Matrix4f boxCurrentPose = currentLeftPose;
        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);
 
        // 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();
 
        // pass sensor value to statechart
        controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
        controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head<3>();
        controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
        controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
        controlInterfaceBuffer.commitWrite();
 
 
        // --------------------------------------------- get MP target ---------------------------------------------
        virtualPose = rtGetControlStruct().boxPose;
        //        Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
 
        // --------------------------------------------- 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;
        tcpTargetPoseRight.block<3, 1>(0, 3) +=
            virtualPose.block<3, 3>(0, 0) * objCom2TCPRightInObjFrame;
 
        // --------------------------------------------- Velocity control ---------------------------------------------
 
        Eigen::Matrix3f diffMatLeft =
            tcpTargetPoseLeft.block<3, 3>(0, 0) * currentLeftPose.block<3, 3>(0, 0).inverse();
        Eigen::Vector3f errorRPYLeft = VirtualRobot::MathTools::eigen3f2rpy(diffMatLeft);
        Eigen::Matrix3f diffMatRight =
            tcpTargetPoseRight.block<3, 3>(0, 0) * currentRightPose.block<3, 3>(0, 0).inverse();
        Eigen::Vector3f errorRPYRight = VirtualRobot::MathTools::eigen3f2rpy(diffMatRight);
 
        Eigen::Vector6f leftTargetVel, rightTargetVel;
        for (size_t i = 0; i < 3; ++i)
        {
            leftTargetVel(i) = KpImpedance(i) * (tcpTargetPoseLeft(i, 3) - currentLeftPose(i, 3)) +
                               KdImpedance(i) * (-currentLeftTwist(i));
            leftTargetVel(i + 3) = KpImpedance(i + 3) * errorRPYLeft(i) +
                                   KdImpedance(i + 3) * (-currentLeftTwist(i + 3));
            rightTargetVel(i) =
                KpImpedance(i) * (tcpTargetPoseRight(i, 3) - currentRightPose(i, 3)) +
                KdImpedance(i) * (-currentRightTwist(i));
            rightTargetVel(i + 3) = KpImpedance(i + 3) * errorRPYRight(i) +
                                    KdImpedance(i + 3) * (-currentRightTwist(i + 3));
        }
 
 
        Eigen::VectorXf leftJointNullSpaceVel =
            cfg->knull * (leftDesiredJointValues - leftqpos) - cfg->dnull * leftqvel +
            cfg->jointLimitAvoidanceKp * leftTCPController->calculateJointLimitAvoidance();
        Eigen::VectorXf leftJointTargetVel =
            calcIK(leftIK, jacobiL, leftTargetVel, leftJointNullSpaceVel);
 
 
        Eigen::VectorXf rightJointNullSpaceVel =
            cfg->knull * (rightDesiredJointValues - rightqpos) - cfg->dnull * rightqvel +
            cfg->jointLimitAvoidanceKp * rightTCPController->calculateJointLimitAvoidance();
        Eigen::VectorXf rightJointTargetVel =
            calcIK(rightIK, jacobiR, rightTargetVel, rightJointNullSpaceVel);
 
        for (size_t i = 0; i < leftTargets.size(); ++i)
        {
            float desiredVel = leftJointTargetVel(i);
            debugOutputData.getWriteBuffer().desired_vels[leftJointNames[i]] = desiredVel;
 
            if (fabs(desiredVel) > cfg->jointVelLimit || isnan(desiredVel))
            {
                desiredVel = 0.0;
            }
 
            leftTargets.at(i)->velocity = desiredVel;
        }
 
        for (size_t i = 0; i < rightTargets.size(); ++i)
        {
            float desiredVel = rightJointTargetVel(i);
            debugOutputData.getWriteBuffer().desired_vels[rightJointNames[i]] = desiredVel;
 
            if (fabs(desiredVel) > cfg->jointVelLimit || isnan(desiredVel))
            {
                desiredVel = 0.0;
            }
 
            rightTargets.at(i)->velocity = desiredVel;
        }
 
 
        // --------------------------------------------- debug output ---------------------------------------------
        debugOutputData.getWriteBuffer().virtualPose_x = virtualPose(0, 3);
        debugOutputData.getWriteBuffer().virtualPose_y = virtualPose(1, 3);
        debugOutputData.getWriteBuffer().virtualPose_z = virtualPose(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().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 = virtualPose(0, 3);
        debugOutputData.getWriteBuffer().dmpBoxPose_y = virtualPose(1, 3);
        debugOutputData.getWriteBuffer().dmpBoxPose_z = virtualPose(2, 3);
 
        VirtualRobot::MathTools::Quaternion dmpQuat =
            VirtualRobot::MathTools::eigen4f2quat(virtualPose);
        debugOutputData.getWriteBuffer().dmpBoxPose_qx = dmpQuat.x;
        debugOutputData.getWriteBuffer().dmpBoxPose_qy = dmpQuat.y;
        debugOutputData.getWriteBuffer().dmpBoxPose_qz = dmpQuat.z;
        debugOutputData.getWriteBuffer().dmpBoxPose_qw = dmpQuat.w;
        debugOutputData.commitWrite();
    }
 
    void
    NJointBimanualObjLevelVelController::learnDMPFromFiles(const Ice::StringSeq& fileNames,
                                                           const Ice::Current&)
    {
        objectDMP->learnDMPFromFiles(fileNames);
    }
 
    void
    NJointBimanualObjLevelVelController::setGoals(const Ice::DoubleSeq& goals,
                                                  const Ice::Current& ice)
    {
        LockGuardType guard(controllerMutex);
        objectDMP->setGoalPoseVec(goals);
        dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
        dmpGoal(0, 3) = goals[0];
        dmpGoal(1, 3) = goals[1];
        dmpGoal(2, 3) = goals[2];
    }
 
    void
    NJointBimanualObjLevelVelController::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;
 
 
        std::vector<double> boxInitialPose;
        for (size_t i = 0; i < 3; ++i)
        {
            boxInitialPose.push_back(boxPosi(i)); //Important: mm -> m
        }
        boxInitialPose.push_back(boxOri.w);
        boxInitialPose.push_back(boxOri.x);
        boxInitialPose.push_back(boxOri.y);
        boxInitialPose.push_back(boxOri.z);
 
        dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
        dmpGoal(0, 3) = goals[0];
        dmpGoal(1, 3) = goals[1];
        dmpGoal(2, 3) = goals[2];
 
        objectDMP->prepareExecution(boxInitialPose, goals);
        objectDMP->canVal = timeDuration;
        objectDMP->config.motionTimeDuration = timeDuration;
 
 
        finished = false;
        dmpStarted = true;
    }
 
    void
    NJointBimanualObjLevelVelController::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 ...";
 
        dmpGoal = VirtualRobot::MathTools::quat2eigen4f(goals[4], goals[5], goals[6], goals[3]);
        dmpGoal(0, 3) = goals[0];
        dmpGoal(1, 3) = goals[1];
        dmpGoal(2, 3) = goals[2];
 
        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
    NJointBimanualObjLevelVelController::setViaPoints(Ice::Double u,
                                                      const Ice::DoubleSeq& viapoint,
                                                      const Ice::Current&)
    {
        //        LockGuardType guard(controllerMutex);
        ARMARX_INFO << "setting via points ";
        objectDMP->setViaPose(u, viapoint);
    }
 
    void
    NJointBimanualObjLevelVelController::removeAllViaPoints(const Ice::Current&)
    {
        objectDMP->removeAllViaPoints();
    }
 
    void
    NJointBimanualObjLevelVelController::setKpImpedance(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().KpImpedance = setpoint;
        interface2rtBuffer.commitWrite();
    }
 
    void
    NJointBimanualObjLevelVelController::setKdImpedance(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().KdImpedance = setpoint;
        interface2rtBuffer.commitWrite();
    }
 
    std::vector<float>
    NJointBimanualObjLevelVelController::getCurrentObjVel(const Ice::Current&)
    {
        Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
        std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
        return tvelvec;
    }
 
    void
    NJointBimanualObjLevelVelController::onPublish(const SensorAndControl&,
                                                   const DebugDrawerInterfacePrx&,
                                                   const DebugObserverInterfacePrx& debugObs)
    {
 
        StringVariantBaseMap datafields;
        auto values = debugOutputData.getUpToDateReadBuffer().desired_vels;
        for (auto& pair : values)
        {
            datafields[pair.first] = new Variant(pair.second);
        }
 
 
        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["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["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["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["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["dmpBoxPose_qx"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qx);
        datafields["dmpBoxPose_qy"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qy);
        datafields["dmpBoxPose_qz"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qz);
        datafields["dmpBoxPose_qw"] =
            new Variant(debugOutputData.getUpToDateReadBuffer().dmpBoxPose_qw);
 
        debugObs->setDebugChannel("BimanualForceController", datafields);
    }
 
    void
    NJointBimanualObjLevelVelController::onInitNJointController()
    {
 
 
        ARMARX_INFO << "init ...";
        runTask("NJointBimanualObjLevelVelController",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            controllerRun();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    NJointBimanualObjLevelVelController::onDisconnectNJointController()
    {
        ARMARX_INFO << "stopped ...";
    }
} // namespace armarx::control::deprecated_njoint_mp_controller::bimanual