DSJointCarryController.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    DSController::ArmarXObjects::DSJointCarryController
 * @author     Mahdi Khoramshahi ( m80 dot khoramshahi at gmail dot com )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "DSJointCarryController.h"
 
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Tools/Gravity.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
namespace armarx::control::ds_controller
{
 
    NJointControllerRegistration<DSJointCarryController>
        registrationControllerDSJointCarryController("DSJointCarryController");
 
    void
    DSJointCarryController::onInitNJointController()
    {
        ARMARX_INFO << "init ...";
        controllerStopRequested = false;
        controllerRunFinished = false;
        runTask("DSJointCarryControllerTask",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted && !controllerStopRequested)
                    {
                        ARMARX_INFO << deactivateSpam(1) << "control fct";
                        if (isControllerActive())
                        {
                            controllerRun();
                        }
                        c.waitForCycleDuration();
                    }
                    controllerRunFinished = true;
                    ARMARX_INFO << "Control Fct finished";
                });
    }
 
    void
    DSJointCarryController::onDisconnectNJointController()
    {
        ARMARX_INFO << "disconnect";
        controllerStopRequested = true;
        while (!controllerRunFinished)
        {
            ARMARX_INFO << deactivateSpam(1) << "waiting for run function";
            usleep(10000);
        }
    }
 
    DSJointCarryController::DSJointCarryController(const RobotUnitPtr& robotUnit,
                                                   const armarx::NJointControllerConfigPtr& config,
                                                   const VirtualRobot::RobotPtr&)
    {
        cfg = DSJointCarryControllerConfigPtr::dynamicCast(config);
        useSynchronizedRtRobot();
 
        VirtualRobot::RobotNodeSetPtr left_ns = rtGetRobot()->getRobotNodeSet("LeftArm");
        VirtualRobot::RobotNodeSetPtr right_ns = rtGetRobot()->getRobotNodeSet("RightArm");
 
        left_jointNames.clear();
        right_jointNames.clear();
 
        ARMARX_CHECK_EXPRESSION(left_ns) << "LeftArm";
        ARMARX_CHECK_EXPRESSION(right_ns) << "RightArm";
 
        // for left arm
        for (size_t i = 0; i < left_ns->getSize(); ++i)
        {
            std::string jointName = left_ns->getNode(i)->getName();
            left_jointNames.push_back(jointName);
            ControlTargetBase* ct = useControlTarget(
                jointName,
                ControlModes::
                    Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
            ARMARX_CHECK_EXPRESSION(ct);
            const SensorValueBase* sv = useSensorValue(jointName);
            ARMARX_CHECK_EXPRESSION(sv);
            auto casted_ct = ct->asA<
                ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
            ARMARX_CHECK_EXPRESSION(casted_ct);
            left_torque_targets.push_back(casted_ct);
 
            const SensorValue1DoFActuatorTorque* torqueSensor =
                sv->asA<SensorValue1DoFActuatorTorque>();
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFGravityTorque* gravityTorqueSensor =
                sv->asA<SensorValue1DoFGravityTorque>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
            if (!torqueSensor)
            {
                ARMARX_WARNING << "No Torque sensor available for " << jointName;
            }
            if (!gravityTorqueSensor)
            {
                ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
            }
 
            left_torqueSensors.push_back(torqueSensor);
            left_gravityTorqueSensors.push_back(gravityTorqueSensor);
            left_velocitySensors.push_back(velocitySensor);
            left_positionSensors.push_back(positionSensor);
        };
 
        // for right arm
        for (size_t i = 0; i < right_ns->getSize(); ++i)
        {
            std::string jointName = right_ns->getNode(i)->getName();
            right_jointNames.push_back(jointName);
            ControlTargetBase* ct = useControlTarget(
                jointName,
                ControlModes::
                    Torque1DoF); // ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Velocity1DoF);
            ARMARX_CHECK_EXPRESSION(ct);
            const SensorValueBase* sv = useSensorValue(jointName);
            ARMARX_CHECK_EXPRESSION(sv);
            auto casted_ct = ct->asA<
                ControlTarget1DoFActuatorTorque>(); // auto casted_ct = ct->asA<ControlTarget1DoFActuatorVelocity>();
            ARMARX_CHECK_EXPRESSION(casted_ct);
            right_torque_targets.push_back(casted_ct);
 
            const SensorValue1DoFActuatorTorque* torqueSensor =
                sv->asA<SensorValue1DoFActuatorTorque>();
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFGravityTorque* gravityTorqueSensor =
                sv->asA<SensorValue1DoFGravityTorque>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
            if (!torqueSensor)
            {
                ARMARX_WARNING << "No Torque sensor available for " << jointName;
            }
            if (!gravityTorqueSensor)
            {
                ARMARX_WARNING << "No Gravity Torque sensor available for " << jointName;
            }
 
            right_torqueSensors.push_back(torqueSensor);
            right_gravityTorqueSensors.push_back(gravityTorqueSensor);
            right_velocitySensors.push_back(velocitySensor);
            right_positionSensors.push_back(positionSensor);
        };
 
 
        const SensorValueBase* svlf = useSensorValue("FT L");
        leftForceTorque = svlf->asA<SensorValueForceTorque>();
        const SensorValueBase* svrf = useSensorValue("FT R");
        rightForceTorque = svrf->asA<SensorValueForceTorque>();
 
        ARMARX_INFO << "Initialized " << left_torque_targets.size() << " targets for the left arm";
        ARMARX_INFO << "Initialized " << right_torque_targets.size()
                    << " targets for the right arm";
 
        left_arm_tcp = left_ns->getTCP();
        right_arm_tcp = right_ns->getTCP();
 
        left_sensor_frame = left_ns->getRobot()->getRobotNode("ArmL_FT");
        right_sensor_frame = right_ns->getRobot()->getRobotNode("ArmR_FT");
 
        left_ik.reset(new VirtualRobot::DifferentialIK(
            left_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
        right_ik.reset(new VirtualRobot::DifferentialIK(
            right_ns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
 
 
        // ?? not sure about this
        DSJointCarryControllerSensorData initSensorData;
        initSensorData.left_tcpPose = left_arm_tcp->getPoseInRootFrame();
        initSensorData.right_tcpPose = right_arm_tcp->getPoseInRootFrame();
        initSensorData.left_force.setZero();
        initSensorData.right_force.setZero();
        initSensorData.currentTime = 0;
        controllerSensorData.reinitAllBuffers(initSensorData);
 
        std::vector<float> left_desiredQuaternionVec = cfg->left_desiredQuaternion;
        ARMARX_CHECK_EQUAL(left_desiredQuaternionVec.size(), 4);
 
        std::vector<float> right_desiredQuaternionVec = cfg->right_desiredQuaternion;
        ARMARX_CHECK_EQUAL(right_desiredQuaternionVec.size(), 4);
 
        left_desiredQuaternion.w() = left_desiredQuaternionVec.at(0);
        left_desiredQuaternion.x() = left_desiredQuaternionVec.at(1);
        left_desiredQuaternion.y() = left_desiredQuaternionVec.at(2);
        left_desiredQuaternion.z() = left_desiredQuaternionVec.at(3);
        right_desiredQuaternion.w() = right_desiredQuaternionVec.at(0);
        right_desiredQuaternion.x() = right_desiredQuaternionVec.at(1);
        right_desiredQuaternion.y() = right_desiredQuaternionVec.at(2);
        right_desiredQuaternion.z() = right_desiredQuaternionVec.at(3);
 
 
        // set initial joint velocities filter
        left_jointVelocity_filtered.resize(left_torque_targets.size());
        left_jointVelocity_filtered.setZero();
        right_jointVelocity_filtered.resize(left_torque_targets.size());
        right_jointVelocity_filtered.setZero();
 
        // do we need to duplicate this?
        DSJointCarryControllerControlData initData;
        for (size_t i = 0; i < 3; ++i)
        {
            initData.leftDesiredLinearVelocity(i) = 0;
            initData.leftDesiredAngularError(i) = 0;
            initData.rightDesiredLinearVelocity(i) = 0;
            initData.rightDesiredAngularError(i) = 0;
        }
        reinitTripleBuffer(initData);
 
        Ctrl2InterfaceData initCtrl2InterfaceData;
        initCtrl2InterfaceData.guardZVel = 0;
        ctrl2InterfaceData.reinitAllBuffers(initCtrl2InterfaceData);
 
        Interface2CtrlData initInterface2CtrlData;
        initInterface2CtrlData.guardToHandInMeter.setZero();
        initInterface2CtrlData.guardToHandInMeter[1] = cfg->guardLength / 2;
        initInterface2CtrlData.guardOriInRobotBase.setIdentity();
        initInterface2CtrlData.desiredGuardOriInRobotBase.w() = cfg->defaultGuardOri[0];
        initInterface2CtrlData.desiredGuardOriInRobotBase.x() = cfg->defaultGuardOri[1];
        initInterface2CtrlData.desiredGuardOriInRobotBase.y() = cfg->defaultGuardOri[2];
        initInterface2CtrlData.desiredGuardOriInRobotBase.z() = cfg->defaultGuardOri[3];
        initInterface2CtrlData.guardRotationStiffness << cfg->defaultRotationStiffness[0],
            cfg->defaultRotationStiffness[1], cfg->defaultRotationStiffness[2];
        initInterface2CtrlData.guardObsAvoidVel.setZero();
        interface2CtrlData.reinitAllBuffers(initInterface2CtrlData);
 
        // initial filter
        left_desiredTorques_filtered.resize(left_torque_targets.size());
        left_desiredTorques_filtered.setZero();
        right_desiredTorques_filtered.resize(right_torque_targets.size());
        right_desiredTorques_filtered.setZero();
 
 
        left_currentTCPLinearVelocity_filtered.setZero();
        right_currentTCPLinearVelocity_filtered.setZero();
 
        left_currentTCPAngularVelocity_filtered.setZero();
        right_currentTCPAngularVelocity_filtered.setZero();
 
        left_tcpDesiredTorque_filtered.setZero();
        right_tcpDesiredTorque_filtered.setZero();
 
 
        smooth_startup = 0;
 
        filterTimeConstant = cfg->filterTimeConstant;
        posiKp = cfg->posiKp;
        v_max = cfg->v_max;
        Damping = cfg->posiDamping;
        torqueLimit = cfg->torqueLimit;
        null_torqueLimit = cfg->NullTorqueLimit;
        oriKp = cfg->oriKp;
        oriDamping = cfg->oriDamping;
 
        // nullspace control
        left_qnullspace.resize(cfg->leftarm_qnullspaceVec.size());
        right_qnullspace.resize(cfg->rightarm_qnullspaceVec.size());
        for (size_t i = 0; i < cfg->leftarm_qnullspaceVec.size(); ++i)
        {
            left_qnullspace(i) = cfg->leftarm_qnullspaceVec[i];
            right_qnullspace(i) = cfg->rightarm_qnullspaceVec[i];
        }
        nullspaceKp = cfg->nullspaceKp;
        nullspaceDamping = cfg->nullspaceDamping;
 
 
        //set GMM parameters ...
        if (cfg->gmmParamsFile == "")
        {
            ARMARX_ERROR << "gmm params file cannot be empty string ...";
        }
        gmmMotionGenerator.reset(new JointCarryGMMMotionGen(cfg->gmmParamsFile));
 
        ARMARX_INFO << "Initialization done";
    }
 
    void
    DSJointCarryController::controllerRun()
    {
        if (!controllerSensorData.updateReadBuffer())
        {
            return;
        }
 
        // receive the measurements
        Eigen::Matrix4f left_currentTCPPose = controllerSensorData.getReadBuffer().left_tcpPose;
        Eigen::Matrix4f right_currentTCPPose = controllerSensorData.getReadBuffer().right_tcpPose;
 
        Eigen::Vector3f left_currentTCPPositionInMeter;
        Eigen::Vector3f right_currentTCPPositionInMeter;
        left_currentTCPPositionInMeter << left_currentTCPPose(0, 3), left_currentTCPPose(1, 3),
            left_currentTCPPose(2, 3);
        right_currentTCPPositionInMeter << right_currentTCPPose(0, 3), right_currentTCPPose(1, 3),
            right_currentTCPPose(2, 3);
        left_currentTCPPositionInMeter = 0.001 * left_currentTCPPositionInMeter;
        right_currentTCPPositionInMeter = 0.001 * right_currentTCPPositionInMeter;
 
        interface2CtrlData.updateReadBuffer();
        Eigen::Quaternionf guard_currentOrientation =
            interface2CtrlData.getReadBuffer().guardOriInRobotBase;
        Eigen::Quaternionf guard_desiredOrientation =
            interface2CtrlData.getReadBuffer().desiredGuardOriInRobotBase;
        Eigen::Vector3f guardToHandInMeter = interface2CtrlData.getReadBuffer().guardToHandInMeter;
        Eigen::Vector3f guardRotationStiffness =
            interface2CtrlData.getReadBuffer().guardRotationStiffness;
        Eigen::Vector3f guardModulatedVel = interface2CtrlData.getReadBuffer().guardObsAvoidVel;
 
        Eigen::Vector3f left_to_right =
            right_currentTCPPositionInMeter - left_currentTCPPositionInMeter;
        left_to_right.normalize();
        // calculate the desired position velocity of the guard
        Eigen::Vector3f guard_currentPosition;
        guard_currentPosition =
            (left_currentTCPPositionInMeter + right_currentTCPPositionInMeter) / 2 +
            guardToHandInMeter;
        gmmMotionGenerator->updateDesiredVelocity(guard_currentPosition,
                                                  cfg->positionErrorTolerance);
        Eigen::Vector3f guardDesiredLinearVelocity = gmmMotionGenerator->guard_desiredVelocity;
 
 
        ctrl2InterfaceData.getWriteBuffer().guardZVel = guardDesiredLinearVelocity[2];
        ctrl2InterfaceData.commitWrite();
 
        guardDesiredLinearVelocity[2] = guardDesiredLinearVelocity[2] + guardModulatedVel[2];
 
        // calculate the desired rotation velocity of the guard
        if (guard_desiredOrientation.coeffs().dot(guard_currentOrientation.coeffs()) < 0.0)
        {
            guard_currentOrientation.coeffs() << -guard_currentOrientation.coeffs();
        }
        Eigen::Quaternionf errorOri = guard_currentOrientation * guard_desiredOrientation.inverse();
        Eigen::AngleAxisf err_axang(errorOri);
        Eigen::Vector3f guard_angular_error = err_axang.axis() * err_axang.angle();
        Eigen::Vector3f guard_desired_angular_vel =
            -1 * guardRotationStiffness.cwiseProduct(guard_angular_error);
 
        // calculate the hand desired linear velocity
        Eigen::Vector3f guard_to_left = left_currentTCPPositionInMeter - guard_currentPosition;
        Eigen::Vector3f leftOriGenLinearVelocity = guard_desired_angular_vel.cross(guard_to_left);
        leftOriGenLinearVelocity -= leftOriGenLinearVelocity.dot(left_to_right) * left_to_right;
        Eigen::Vector3f leftDesiredLinearVelocity =
            leftOriGenLinearVelocity + guardDesiredLinearVelocity;
 
        if (leftDesiredLinearVelocity.norm() > cfg->handVelLimit)
        {
            leftDesiredLinearVelocity =
                cfg->handVelLimit * leftDesiredLinearVelocity / leftDesiredLinearVelocity.norm();
        }
 
        Eigen::Vector3f guard_to_right = right_currentTCPPositionInMeter - guard_currentPosition;
        Eigen::Vector3f rightOriGenLinearVelocity = guard_desired_angular_vel.cross(guard_to_right);
        rightOriGenLinearVelocity -= rightOriGenLinearVelocity.dot(left_to_right) * left_to_right;
        Eigen::Vector3f rightDesiredLinearVelocity =
            rightOriGenLinearVelocity + guardDesiredLinearVelocity;
 
        if (rightDesiredLinearVelocity.norm() > cfg->handVelLimit)
        {
            rightDesiredLinearVelocity =
                cfg->handVelLimit * rightDesiredLinearVelocity / rightDesiredLinearVelocity.norm();
        }
 
        // calculate the desired orientation of the hand
        Eigen::Matrix3f left_currentRotMat = left_currentTCPPose.block<3, 3>(0, 0);
        Eigen::Matrix3f right_currentRotMat = right_currentTCPPose.block<3, 3>(0, 0);
        Eigen::Matrix3f left_desiredRotMat = left_desiredQuaternion.normalized().toRotationMatrix();
        Eigen::Matrix3f right_desiredRotMat =
            right_desiredQuaternion.normalized().toRotationMatrix();
        Eigen::Matrix3f left_orientationError = left_currentRotMat * left_desiredRotMat.inverse();
        Eigen::Matrix3f right_orientationError =
            right_currentRotMat * right_desiredRotMat.inverse();
        Eigen::Quaternionf left_diffQuaternion(left_orientationError);
        Eigen::Quaternionf right_diffQuaternion(right_orientationError);
        Eigen::AngleAxisf left_angleAxis(left_diffQuaternion);
        Eigen::AngleAxisf right_angleAxis(right_diffQuaternion);
        Eigen::Vector3f left_tcpDesiredAngularError =
            left_angleAxis.angle() * left_angleAxis.axis();
        Eigen::Vector3f right_tcpDesiredAngularError =
            right_angleAxis.angle() * right_angleAxis.axis();
 
        // writing to the buffer
        LockGuardType guard{controlDataMutex};
        getWriterControlStruct().leftDesiredLinearVelocity = leftDesiredLinearVelocity;
        getWriterControlStruct().leftDesiredAngularError = left_tcpDesiredAngularError;
        getWriterControlStruct().rightDesiredLinearVelocity = rightDesiredLinearVelocity;
        getWriterControlStruct().rightDesiredAngularError = right_tcpDesiredAngularError;
        writeControlStruct();
 
        debugCtrlDataInfo.getWriteBuffer().leftDesiredLinearVelocity = leftDesiredLinearVelocity;
        debugCtrlDataInfo.getWriteBuffer().rightDesiredLinearVelocity = rightDesiredLinearVelocity;
        debugCtrlDataInfo.commitWrite();
    }
 
    void
    DSJointCarryController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
                                  const IceUtil::Time& timeSinceLastIteration)
    {
        /* get sensor data */
        double deltaT = timeSinceLastIteration.toSecondsDouble();
 
        Eigen::Matrix4f leftSensorFrame = left_sensor_frame->getPoseInRootFrame();
        Eigen::Matrix4f rightSensorFrame = right_sensor_frame->getPoseInRootFrame();
        Eigen::Vector3f left_forceInRoot =
            leftSensorFrame.block<3, 3>(0, 0) * (leftForceTorque->force);
        Eigen::Vector3f right_forceInRoot =
            rightSensorFrame.block<3, 3>(0, 0) * (rightForceTorque->force);
        left_forceInRoot(2) = left_forceInRoot(2); // - 4 + 8.5;
        right_forceInRoot(2) = right_forceInRoot(2); // + 30 - 5.2;
        Eigen::Matrix4f left_currentTCPPose = left_arm_tcp->getPoseInRootFrame();
        Eigen::Matrix4f right_currentTCPPose = right_arm_tcp->getPoseInRootFrame();
 
 
        Eigen::MatrixXf left_jacobi = left_ik->getJacobianMatrix(
            left_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
        Eigen::MatrixXf right_jacobi = right_ik->getJacobianMatrix(
            right_arm_tcp, VirtualRobot::IKSolver::CartesianSelection::All);
 
        Eigen::VectorXf left_qpos;
        Eigen::VectorXf left_qvel;
 
        Eigen::VectorXf right_qpos;
        Eigen::VectorXf right_qvel;
 
        left_qpos.resize(left_positionSensors.size());
        left_qvel.resize(left_velocitySensors.size());
 
        right_qpos.resize(right_positionSensors.size());
        right_qvel.resize(right_velocitySensors.size());
 
        int jointDim = left_positionSensors.size();
 
        for (size_t i = 0; i < left_velocitySensors.size(); ++i)
        {
            left_qpos(i) = left_positionSensors[i]->position;
            left_qvel(i) = left_velocitySensors[i]->velocity;
 
            right_qpos(i) = right_positionSensors[i]->position;
            right_qvel(i) = right_velocitySensors[i]->velocity;
        }
 
        Eigen::VectorXf left_tcptwist = left_jacobi * left_qvel;
        Eigen::VectorXf right_tcptwist = right_jacobi * right_qvel;
 
        Eigen::Vector3f left_currentTCPLinearVelocity;
        Eigen::Vector3f right_currentTCPLinearVelocity;
        Eigen::Vector3f left_currentTCPAngularVelocity;
        Eigen::Vector3f right_currentTCPAngularVelocity;
        left_currentTCPLinearVelocity << 0.001 * left_tcptwist(0), 0.001 * left_tcptwist(1),
            0.001 * left_tcptwist(2);
        right_currentTCPLinearVelocity << 0.001 * right_tcptwist(0), 0.001 * right_tcptwist(1),
            0.001 * right_tcptwist(2);
        left_currentTCPAngularVelocity << left_tcptwist(3), left_tcptwist(4), left_tcptwist(5);
        right_currentTCPAngularVelocity << right_tcptwist(3), right_tcptwist(4), right_tcptwist(5);
        double filterFactor = deltaT / (filterTimeConstant + deltaT);
        left_currentTCPLinearVelocity_filtered =
            (1 - filterFactor) * left_currentTCPLinearVelocity_filtered +
            filterFactor * left_currentTCPLinearVelocity;
        right_currentTCPLinearVelocity_filtered =
            (1 - filterFactor) * right_currentTCPLinearVelocity_filtered +
            filterFactor * right_currentTCPLinearVelocity;
        left_currentTCPAngularVelocity_filtered =
            (1 - filterFactor) * left_currentTCPAngularVelocity_filtered +
            left_currentTCPAngularVelocity;
        right_currentTCPAngularVelocity_filtered =
            (1 - filterFactor) * right_currentTCPAngularVelocity_filtered +
            right_currentTCPAngularVelocity;
        left_jointVelocity_filtered =
            (1 - filterFactor) * left_jointVelocity_filtered + filterFactor * left_qvel;
        right_jointVelocity_filtered =
            (1 - filterFactor) * right_jointVelocity_filtered + filterFactor * right_qvel;
 
        controllerSensorData.getWriteBuffer().left_tcpPose = left_currentTCPPose;
        controllerSensorData.getWriteBuffer().right_tcpPose = right_currentTCPPose;
        controllerSensorData.getWriteBuffer().left_force = left_forceInRoot;
        controllerSensorData.getWriteBuffer().right_force = right_forceInRoot;
        controllerSensorData.getWriteBuffer().currentTime += deltaT;
        controllerSensorData.commitWrite();
 
 
        Eigen::Vector3f leftDesiredLinearVelocity = rtGetControlStruct().leftDesiredLinearVelocity;
        Eigen::Vector3f rightDesiredLinearVelocity =
            rtGetControlStruct().rightDesiredLinearVelocity;
        Eigen::Vector3f left_tcpDesiredAngularError = rtGetControlStruct().leftDesiredAngularError;
        Eigen::Vector3f right_tcpDesiredAngularError =
            rtGetControlStruct().rightDesiredAngularError;
 
        // computing the task-specific forces
        Eigen::Vector3f left_DS_force =
            -(left_currentTCPLinearVelocity_filtered - leftDesiredLinearVelocity);
        Eigen::Vector3f right_DS_force =
            -(right_currentTCPLinearVelocity_filtered - rightDesiredLinearVelocity);
        for (int i = 0; i < 3; ++i)
        {
            left_DS_force(i) = left_DS_force(i) * Damping[i];
            right_DS_force(i) = right_DS_force(i) * Damping[i];
            left_DS_force(i) = deadzone(left_DS_force(i), 0.1, 100);
            right_DS_force(i) = deadzone(right_DS_force(i), 0.1, 100);
        }
 
        Eigen::Vector3f left_tcpDesiredTorque =
            -oriKp * left_tcpDesiredAngularError -
            oriDamping * left_currentTCPAngularVelocity_filtered;
        Eigen::Vector3f right_tcpDesiredTorque =
            -oriKp * right_tcpDesiredAngularError -
            oriDamping * right_currentTCPAngularVelocity_filtered;
 
        left_tcpDesiredTorque_filtered = (1 - filterFactor) * left_tcpDesiredTorque_filtered +
                                         filterFactor * left_tcpDesiredTorque;
        right_tcpDesiredTorque_filtered = (1 - filterFactor) * right_tcpDesiredTorque_filtered +
                                          filterFactor * right_tcpDesiredTorque;
 
        Eigen::Vector6f left_tcpDesiredWrench;
        Eigen::Vector6f right_tcpDesiredWrench;
 
        // times 0.001 because Jacobi matrix is in mm and radian.
        left_tcpDesiredWrench << 0.001 * left_DS_force, left_tcpDesiredTorque_filtered;
        right_tcpDesiredWrench << 0.001 * right_DS_force, right_tcpDesiredTorque_filtered;
 
        //    left_tcpDesiredWrench(2) += 0.001 * cfg->guardGravity / 4;
        //    right_tcpDesiredWrench(2) += 0.001 * cfg->guardGravity / 4;
 
 
        // calculate the null-spcae torque
        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(jointDim, jointDim);
        float lambda = 0.2;
        Eigen::MatrixXf left_jtpinv =
            left_ik->computePseudoInverseJacobianMatrix(left_jacobi.transpose(), lambda);
        Eigen::MatrixXf right_jtpinv =
            right_ik->computePseudoInverseJacobianMatrix(right_jacobi.transpose(), lambda);
        Eigen::VectorXf left_nullqerror = left_qpos - left_qnullspace;
        Eigen::VectorXf right_nullqerror = right_qpos - right_qnullspace;
 
        for (int i = 0; i < left_nullqerror.size(); ++i)
        {
            if (fabs(left_nullqerror(i)) < 0.09)
            {
                left_nullqerror(i) = 0;
            }
 
            if (fabs(right_nullqerror(i)) < 0.09)
            {
                right_nullqerror(i) = 0;
            }
        }
 
        Eigen::VectorXf left_nullspaceTorque =
            -nullspaceKp * left_nullqerror - nullspaceDamping * left_jointVelocity_filtered;
        Eigen::VectorXf right_nullspaceTorque =
            -nullspaceKp * right_nullqerror - nullspaceDamping * right_jointVelocity_filtered;
        Eigen::VectorXf left_projectedNullTorque =
            (I - left_jacobi.transpose() * left_jtpinv) * left_nullspaceTorque;
        Eigen::VectorXf right_projectedNullTorque =
            (I - right_jacobi.transpose() * right_jtpinv) * right_nullspaceTorque;
        float left_nulltorque_norm = left_projectedNullTorque.norm();
        float right_nulltorque_norm = right_projectedNullTorque.norm();
        if (left_nulltorque_norm > null_torqueLimit)
        {
            left_projectedNullTorque =
                (null_torqueLimit / left_nulltorque_norm) * left_projectedNullTorque;
        }
        if (right_nulltorque_norm > null_torqueLimit)
        {
            right_projectedNullTorque =
                (null_torqueLimit / right_nulltorque_norm) * right_projectedNullTorque;
        }
 
        Eigen::VectorXf left_jointDesiredTorques =
            left_jacobi.transpose() * left_tcpDesiredWrench + left_projectedNullTorque;
        Eigen::VectorXf right_jointDesiredTorques =
            right_jacobi.transpose() * right_tcpDesiredWrench + right_projectedNullTorque;
        for (size_t i = 0; i < left_torque_targets.size(); ++i)
        {
            float desiredTorque = smooth_startup * left_jointDesiredTorques(i);
            desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
            left_desiredTorques_filtered(i) =
                (1 - cfg->TorqueFilterConstant) * left_desiredTorques_filtered(i) +
                cfg->TorqueFilterConstant * desiredTorque;
 
            std::string names = left_jointNames[i] + "_desiredTorques";
            debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
            names = names + "_filtered";
            debugDataInfo.getWriteBuffer().desired_torques[names] = left_desiredTorques_filtered(i);
 
            if (fabs(left_desiredTorques_filtered(i)) > torqueLimit)
            {
                left_torque_targets.at(i)->torque = 0;
            }
            else
            {
                left_torque_targets.at(i)->torque =
                    left_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
            }
        }
 
        for (size_t i = 0; i < right_torque_targets.size(); ++i)
        {
            float desiredTorque = smooth_startup * right_jointDesiredTorques(i);
            desiredTorque = deadzone(desiredTorque, cfg->desiredTorqueDisturbance, torqueLimit);
            right_desiredTorques_filtered(i) =
                (1 - cfg->TorqueFilterConstant) * right_desiredTorques_filtered(i) +
                cfg->TorqueFilterConstant * desiredTorque;
 
            std::string names = right_jointNames[i] + "_desiredTorques";
            debugDataInfo.getWriteBuffer().desired_torques[names] = desiredTorque;
            names = names + "_filtered";
            debugDataInfo.getWriteBuffer().desired_torques[names] =
                right_desiredTorques_filtered(i);
 
            if (fabs(right_desiredTorques_filtered(i)) > torqueLimit)
            {
                right_torque_targets.at(i)->torque = 0;
            }
            else
            {
                right_torque_targets.at(i)->torque =
                    right_desiredTorques_filtered(i); // targets.at(i)->velocity = desiredVelocity;
            }
        }
 
        smooth_startup = smooth_startup + 0.001 * (1.1 - smooth_startup);
        smooth_startup = (smooth_startup > 1) ? 1 : smooth_startup;
        smooth_startup = (smooth_startup < 0) ? 0 : smooth_startup;
 
        debugDataInfo.commitWrite();
    }
 
    void
    DSJointCarryController::setGuardOrientation(const Ice::FloatSeq& guardOrientationInRobotBase,
                                                const Ice::Current&)
    {
        Eigen::Quaternionf guardOri;
        guardOri.w() = guardOrientationInRobotBase[0];
        guardOri.x() = guardOrientationInRobotBase[1];
        guardOri.y() = guardOrientationInRobotBase[2];
        guardOri.z() = guardOrientationInRobotBase[3];
 
        LockGuardType guard{interface2CtrlDataMutex};
        interface2CtrlData.getWriteBuffer().guardOriInRobotBase = guardOri;
        interface2CtrlData.commitWrite();
    }
 
    void
    DSJointCarryController::setGuardInHandPosition(const Ice::FloatSeq& guardPositionToHandInMeter,
                                                   const Ice::Current&)
    {
        Eigen::Vector3f guardPosi;
        guardPosi << guardPositionToHandInMeter[0], guardPositionToHandInMeter[1],
            guardPositionToHandInMeter[2];
 
        LockGuardType guard{interface2CtrlDataMutex};
        interface2CtrlData.getWriteBuffer().guardToHandInMeter = guardPosi;
        interface2CtrlData.commitWrite();
    }
 
    void
    DSJointCarryController::setDesiredGuardOri(const Ice::FloatSeq& desiredOrientationInRobotBase,
                                               const Ice::Current&)
    {
        Eigen::Quaternionf guardOri;
        guardOri.w() = desiredOrientationInRobotBase[0];
        guardOri.x() = desiredOrientationInRobotBase[1];
        guardOri.y() = desiredOrientationInRobotBase[2];
        guardOri.z() = desiredOrientationInRobotBase[3];
 
        LockGuardType guard{interface2CtrlDataMutex};
        interface2CtrlData.getWriteBuffer().desiredGuardOriInRobotBase = guardOri;
        interface2CtrlData.commitWrite();
    }
 
    void
    DSJointCarryController::setRotationStiffness(const Ice::FloatSeq& rotationStiffness,
                                                 const Ice::Current&)
    {
        Eigen::Vector3f rotStiffness;
        rotStiffness << rotationStiffness[0], rotStiffness[1], rotStiffness[2];
 
        LockGuardType guard{interface2CtrlDataMutex};
        interface2CtrlData.getWriteBuffer().guardRotationStiffness = rotStiffness;
        interface2CtrlData.commitWrite();
    }
 
    void
    DSJointCarryController::setGuardObsAvoidVel(const Ice::FloatSeq& guardObsAvoidVel,
                                                const Ice::Current&)
    {
        LockGuardType guard{interface2CtrlDataMutex};
        interface2CtrlData.getWriteBuffer().guardObsAvoidVel(0) = guardObsAvoidVel[0];
        interface2CtrlData.getWriteBuffer().guardObsAvoidVel(1) = guardObsAvoidVel[1];
        interface2CtrlData.getWriteBuffer().guardObsAvoidVel(2) = guardObsAvoidVel[2];
        interface2CtrlData.commitWrite();
    }
 
    float
    DSJointCarryController::getGMMVel(const Ice::Current&)
    {
        float gmmZVel = ctrl2InterfaceData.getUpToDateReadBuffer().guardZVel;
        return gmmZVel;
    }
 
    float
    DSJointCarryController::deadzone(float input, float disturbance, float threshold)
    {
        if (input > 0)
        {
            input = input - disturbance;
            input = (input < 0) ? 0 : input;
            input = (input > threshold) ? threshold : input;
        }
        else if (input < 0)
        {
            input = input + disturbance;
            input = (input > 0) ? 0 : input;
            input = (input < -threshold) ? -threshold : input;
        }
 
 
        return input;
    }
 
    Eigen::Quaternionf
    DSJointCarryController::quatSlerp(double t,
                                      const Eigen::Quaternionf& q0,
                                      const Eigen::Quaternionf& q1)
    {
        double cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
 
 
        Eigen::Quaternionf q1x = q1;
        if (cosHalfTheta < 0)
        {
            q1x.w() = -q1.w();
            q1x.x() = -q1.x();
            q1x.y() = -q1.y();
            q1x.z() = -q1.z();
        }
 
 
        if (fabs(cosHalfTheta) >= 1.0)
        {
            return q0;
        }
 
        double halfTheta = acos(cosHalfTheta);
        double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
 
 
        Eigen::Quaternionf result;
        if (fabs(sinHalfTheta) < 0.001)
        {
            result.w() = (1 - t) * q0.w() + t * q1x.w();
            result.x() = (1 - t) * q0.x() + t * q1x.x();
            result.y() = (1 - t) * q0.y() + t * q1x.y();
            result.z() = (1 - t) * q0.z() + t * q1x.z();
        }
        else
        {
            double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
            double ratioB = sin(t * halfTheta) / sinHalfTheta;
 
            result.w() = ratioA * q0.w() + ratioB * q1x.w();
            result.x() = ratioA * q0.x() + ratioB * q1x.x();
            result.y() = ratioA * q0.y() + ratioB * q1x.y();
            result.z() = ratioA * q0.z() + ratioB * q1x.z();
        }
 
        return result;
    }
 
    void
    DSJointCarryController::onPublish(const SensorAndControl&,
                                      const DebugDrawerInterfacePrx&,
                                      const DebugObserverInterfacePrx& debugObs)
    {
 
        StringVariantBaseMap datafields;
        auto values = debugDataInfo.getUpToDateReadBuffer().desired_torques;
        for (auto& pair : values)
        {
            datafields[pair.first] = new Variant(pair.second);
        }
 
 
        datafields["leftDesiredLinearVelocity_x"] =
            new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[0]);
        datafields["leftDesiredLinearVelocity_y"] =
            new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[1]);
        datafields["leftDesiredLinearVelocity_z"] =
            new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().leftDesiredLinearVelocity[2]);
        datafields["rightDesiredLinearVelocity_x"] =
            new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[0]);
        datafields["rightDesiredLinearVelocity_y"] =
            new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[1]);
        datafields["rightDesiredLinearVelocity_z"] =
            new Variant(debugCtrlDataInfo.getUpToDateReadBuffer().rightDesiredLinearVelocity[2]);
        debugObs->setDebugChannel("DSJointCarry", datafields);
    }
 
    void
    DSJointCarryController::rtPreActivateController()
    {
    }
 
    void
    DSJointCarryController::rtPostDeactivateController()
    {
    }
} // namespace armarx::control::ds_controller