KeypointsAdmittanceController.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    ...
 * @author     Jianfeng Gao ( jianfeng dot gao at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "KeypointsAdmittanceController.h"
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/time/CycleUtil.h>
 
#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>
 
namespace armarx::control::njoint_controller::task_space
{
    NJointControllerRegistration<NJointKeypointsAdmittanceController>
        registrationControllerNJointKeypointsAdmittanceController(
            "NJointKeypointsAdmittanceController");
 
    NJointKeypointsAdmittanceController::NJointKeypointsAdmittanceController(
        const RobotUnitPtr& robotUnit,
        const NJointControllerConfigPtr& config,
        const VirtualRobot::RobotPtr&)
    {
        ARMARX_INFO << "--------------------------------------- creating keypoints admittance "
                       "controller ---------------------------------------";
        ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
 
        ARMARX_CHECK_EXPRESSION(cfg);
        ARMARX_CHECK_EXPRESSION(robotUnit);
        ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty());
        useSynchronizedRtRobot();
        kinematicChainName = cfg->nodeSetName;
 
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
        ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
 
        jointNames.clear();
        for (size_t i = 0; i < rns->getSize(); ++i)
        {
            std::string jointName = rns->getNode(i)->getName();
            jointNames.push_back(jointName);
            ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF);
            ARMARX_CHECK_EXPRESSION(ct);
            const SensorValueBase* sv = useSensorValue(jointName);
            ARMARX_CHECK_EXPRESSION(sv);
            auto casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>();
            ARMARX_CHECK_EXPRESSION(casted_ct);
            targets.push_back(casted_ct);
 
            const SensorValue1DoFActuatorTorque* torqueSensor =
                sv->asA<SensorValue1DoFActuatorTorque>();
            const SensorValue1DoFActuatorVelocity* velocitySensor =
                sv->asA<SensorValue1DoFActuatorVelocity>();
            const SensorValue1DoFActuatorPosition* positionSensor =
                sv->asA<SensorValue1DoFActuatorPosition>();
            if (!torqueSensor)
            {
                ARMARX_WARNING << "No Torque sensor available for " << jointName;
            }
            if (!velocitySensor)
            {
                ARMARX_WARNING << "No velocity sensor available for " << jointName;
            }
            if (!positionSensor)
            {
                ARMARX_WARNING << "No position sensor available for " << jointName;
            }
 
            torqueSensors.push_back(torqueSensor);
            velocitySensors.push_back(velocitySensor);
            positionSensors.push_back(positionSensor);
        };
        configFileName = cfg->controlParamJsonFile;
        controller.initialize(rns, configFileName);
        ftsensor.initialize(rns, robotUnit, configFileName);
 
        common::FTSensor::FTBufferData ftData;
        ftData.ftFilter = ftsensor.s.ftFilter;
        ftData.deadZoneForce = ftsensor.s.deadZoneForce;
        ftData.deadZoneTorque = ftsensor.s.deadZoneTorque;
        ftData.timeLimit = ftsensor.s.timeLimit;
        ftData.tcpMass = ftsensor.s.tcpMass;
        ftData.tcpCoMInFTSensorFrame = ftsensor.s.tcpCoMInFTSensorFrame;
        ftData.enableTCPGravityCompensation = ftsensor.s.enableTCPGravityCompensation;
        ftData.forceBaseline = ftsensor.s.forceBaseline;
        ftData.torqueBaseline = ftsensor.s.torqueBaseline;
        ftSensorBuffer.reinitAllBuffers(ftData);
 
        law::KeypointsAdmittanceController::Config configData;
        configData.kpImpedance = controller.s.kpImpedance;
        configData.kdImpedance = controller.s.kdImpedance;
        configData.kpAdmittance = controller.s.kpAdmittance;
        configData.kdAdmittance = controller.s.kdAdmittance;
        configData.kmAdmittance = controller.s.kmAdmittance;
 
        configData.kpNullspace = controller.s.kpNullspace;
        configData.kdNullspace = controller.s.kdNullspace;
 
        configData.currentForceTorque.setZero();
        configData.desiredNullspaceJointAngles = controller.s.desiredNullspaceJointAngles;
 
        configData.torqueLimit = controller.s.torqueLimit;
        configData.qvelFilter = controller.s.qvelFilter;
 
        configData.numPoints = controller.s.numPoints;
        configData.keypointKp = controller.s.keypointKp;
        configData.keypointKd = controller.s.keypointKd;
 
        configData.currentKeypointPosition = controller.s.currentKeypointPosition;
        configData.desiredKeypointPosition = controller.s.currentKeypointPosition;
        configData.desiredKeypointVelocity = controller.s.desiredKeypointVelocity;
        configData.keypointPositionFilter = controller.s.keypointPositionFilter;
        configData.keypointVelocityFilter = controller.s.keypointVelocityFilter;
 
        configData.isRigid = controller.s.isRigid;
        configData.fixedTranslation = controller.s.fixedTranslation;
        reinitTripleBuffer(configData);
        ARMARX_INFO << "--------------------------------------- done "
                       "---------------------------------------";
    }
 
    std::string
    NJointKeypointsAdmittanceController::getClassName(const Ice::Current&) const
    {
        return "NJointKeypointsAdmittanceController";
    }
 
    std::string
    NJointKeypointsAdmittanceController::getKinematicChainName(const Ice::Current&)
    {
        return kinematicChainName;
    }
 
    void
    NJointKeypointsAdmittanceController::onInitNJointController()
    {
        runTask("NJointTaskspaceAdmittanceAdditionalTask",
                [&]
                {
                    CycleUtil c(1);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    ARMARX_IMPORTANT
                        << "Create a new thread alone NJointKeypointsAdmittanceController";
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive() and rtReady.load())
                        {
                            additionalTask();
                        }
                        c.waitForCycleDuration();
                    }
                });
    }
 
    void
    NJointKeypointsAdmittanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
                                               const IceUtil::Time& timeSinceLastIteration)
    {
        rtUpdateControlStruct();
        bool valid = controller.updateControlStatus(rtGetControlStruct(),
                                                    timeSinceLastIteration,
                                                    torqueSensors,
                                                    velocitySensors,
                                                    positionSensors);
 
        if (rtFirstRun.load())
        {
            rtFirstRun.store(false);
            rtReady.store(false);
            ftsensor.reset();
            controller.firstRun();
        }
        else
        {
            ftsensor.compensateTCPGravity(ftSensorBuffer.getReadBuffer());
            if (!rtReady.load())
            {
                if (ftsensor.calibrate(controller.s.deltaT))
                {
                    rtReady.store(true);
                }
            }
            else
            {
                if (valid)
                {
                    controller.s.currentForceTorque =
                        ftsensor.getFilteredForceTorque(ftSensorBuffer.getReadBuffer());
                }
            }
        }
 
        controller.run(rtReady.load(), targets);
 
 
        /// for debug output
        {
            controlStatusBuffer.getWriteBuffer().currentPose = controller.s.currentPose;
            controlStatusBuffer.getWriteBuffer().currentTwist = controller.s.currentTwist * 1000.0f;
            controlStatusBuffer.getWriteBuffer().deltaT = controller.s.deltaT;
 
            controlStatusBuffer.getWriteBuffer().kpImpedance = controller.s.kpImpedance;
            controlStatusBuffer.getWriteBuffer().kdImpedance = controller.s.kdImpedance;
            controlStatusBuffer.getWriteBuffer().forceImpedance = controller.s.forceImpedance;
            controlStatusBuffer.getWriteBuffer().currentForceTorque =
                controller.s.currentForceTorque;
            controlStatusBuffer.getWriteBuffer().pointTrackingForce =
                controller.s.pointTrackingForce;
 
            controlStatusBuffer.getWriteBuffer().virtualPose = controller.s.virtualPose;
            controlStatusBuffer.getWriteBuffer().virtualVel = controller.s.virtualVel;
            controlStatusBuffer.getWriteBuffer().virtualAcc = controller.s.virtualAcc;
 
            controlStatusBuffer.getWriteBuffer().desiredPose = controller.s.desiredPose;
            controlStatusBuffer.getWriteBuffer().desiredVel = controller.s.desiredVel;
            controlStatusBuffer.getWriteBuffer().desiredAcc = controller.s.desiredAcc;
 
            controlStatusBuffer.getWriteBuffer().desiredKeypointPosition =
                controller.s.desiredKeypointPosition;
            controlStatusBuffer.getWriteBuffer().currentKeypointPosition =
                controller.s.currentKeypointPosition;
            controlStatusBuffer.getWriteBuffer().currentKeypointVelocity =
                controller.s.currentKeypointVelocity;
            controlStatusBuffer.getWriteBuffer().filteredKeypointPosition =
                controller.s.filteredKeypointPosition;
 
            controlStatusBuffer.getWriteBuffer().desiredJointTorques =
                controller.s.desiredJointTorques;
            controlStatusBuffer.commitWrite();
        }
    }
 
    void
    NJointKeypointsAdmittanceController::onPublish(const SensorAndControl&,
                                                   const DebugDrawerInterfacePrx&,
                                                   const DebugObserverInterfacePrx& debugObs)
    {
        StringVariantBaseMap datafields;
        auto values = controlStatusBuffer.getUpToDateReadBuffer().desiredJointTorques;
        for (int i = 0; i < values.size(); i++)
        {
            datafields["torque_" + std::to_string(i)] = new Variant(values(i));
        }
 
        int dim = 3 * controlStatusBuffer.getUpToDateReadBuffer().numPoints;
 
        Eigen::Matrix4f virtualPose = controlStatusBuffer.getUpToDateReadBuffer().virtualPose;
        Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(virtualPose);
        datafields["virtualPose_x"] = new Variant(virtualPose(0, 3));
        datafields["virtualPose_y"] = new Variant(virtualPose(1, 3));
        datafields["virtualPose_z"] = new Variant(virtualPose(2, 3));
        datafields["virtualPose_rx"] = new Variant(rpy(0));
        datafields["virtualPose_ry"] = new Variant(rpy(1));
        datafields["virtualPose_rz"] = new Variant(rpy(2));
 
        Eigen::Matrix4f desiredPose = controlStatusBuffer.getUpToDateReadBuffer().desiredPose;
        Eigen::Vector3f rpyDesired = VirtualRobot::MathTools::eigen4f2rpy(desiredPose);
        datafields["desiredPose_x"] = new Variant(desiredPose(0, 3));
        datafields["desiredPose_y"] = new Variant(desiredPose(1, 3));
        datafields["desiredPose_z"] = new Variant(desiredPose(2, 3));
        datafields["desiredPose_rx"] = new Variant(rpyDesired(0));
        datafields["desiredPose_ry"] = new Variant(rpyDesired(1));
        datafields["desiredPose_rz"] = new Variant(rpyDesired(2));
 
        datafields["virtualVel_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(0));
        datafields["virtualVel_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(1));
        datafields["virtualVel_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(2));
        datafields["virtualVel_rx"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(3));
        datafields["virtualVel_ry"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(4));
        datafields["virtualVel_rz"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualVel(5));
 
        datafields["virtualAcc_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(0));
        datafields["virtualAcc_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(1));
        datafields["virtualAcc_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(2));
        datafields["virtualAcc_rx"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(3));
        datafields["virtualAcc_ry"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(4));
        datafields["virtualAcc_rz"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().virtualAcc(5));
 
        datafields["desiredTwist_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredVel(0));
        datafields["desiredTwist_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredVel(1));
        datafields["desiredTwist_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredVel(2));
        datafields["desiredTwist_rx"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredVel(3));
        datafields["desiredTwist_ry"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredVel(4));
        datafields["desiredTwist_rz"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredVel(5));
 
        datafields["desiredAcc_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredAcc(0));
        datafields["desiredAcc_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredAcc(1));
        datafields["desiredAcc_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredAcc(2));
        datafields["desiredAcc_rx"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredAcc(3));
        datafields["desiredAcc_ry"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredAcc(4));
        datafields["desiredAcc_rz"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredAcc(5));
 
        datafields["kpImpedance_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(0));
        datafields["kpImpedance_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(1));
        datafields["kpImpedance_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(2));
        datafields["kpImpedance_rx"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(3));
        datafields["kpImpedance_ry"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(4));
        datafields["kpImpedance_rz"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kpImpedance(5));
 
        datafields["kdImpedance_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(0));
        datafields["kdImpedance_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(1));
        datafields["kdImpedance_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(2));
        datafields["kdImpedance_rx"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(3));
        datafields["kdImpedance_ry"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(4));
        datafields["kdImpedance_rz"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().kdImpedance(5));
 
        datafields["forceImpedance_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(0));
        datafields["forceImpedance_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(1));
        datafields["forceImpedance_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(2));
        datafields["forceImpedance_rx"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(3));
        datafields["forceImpedance_ry"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(4));
        datafields["forceImpedance_rz"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().forceImpedance(5));
 
        datafields["currentForceTorque_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(0));
        datafields["currentForceTorque_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(1));
        datafields["currentForceTorque_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(2));
        datafields["currentForceTorque_rx"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(3));
        datafields["currentForceTorque_ry"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(4));
        datafields["currentForceTorque_rz"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentForceTorque(5));
 
        datafields["pointTrackingForce_x"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(0));
        datafields["pointTrackingForce_y"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(1));
        datafields["pointTrackingForce_z"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(2));
        datafields["pointTrackingForce_rx"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(3));
        datafields["pointTrackingForce_ry"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(4));
        datafields["pointTrackingForce_rz"] =
            new Variant(controlStatusBuffer.getUpToDateReadBuffer().pointTrackingForce(5));
 
        for (int i = 0; i < dim; i++)
        {
            datafields["desiredKeypointPosition_" + std::to_string(i)] =
                new Variant(controlStatusBuffer.getUpToDateReadBuffer().desiredKeypointPosition(i));
            datafields["currentKeypointPosition_" + std::to_string(i)] =
                new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointPosition(i));
            datafields["currentKeypointVelocity_" + std::to_string(i)] =
                new Variant(controlStatusBuffer.getUpToDateReadBuffer().currentKeypointVelocity(i));
            datafields["filteredKeypointPosition_" + std::to_string(i)] = new Variant(
                controlStatusBuffer.getUpToDateReadBuffer().filteredKeypointPosition(i));
        }
        debugObs->setDebugChannel("NJointKeypointsAdmittanceController", datafields);
    }
 
    void
    NJointKeypointsAdmittanceController::reconfigureController(const std::string& filename,
                                                               const Ice::Current&)
    {
        law::KeypointsAdmittanceController::Config config = controller.reconfigure(filename);
        reinitTripleBuffer(config);
 
        common::FTSensor::FTBufferData ftConfig = ftsensor.reconfigure(filename);
        ftSensorBuffer.reinitAllBuffers(ftConfig);
    }
 
    void
    NJointKeypointsAdmittanceController::setKeypointsParameters(const std::string& name,
                                                                const Eigen::VectorXf& value,
                                                                const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        if (name == "target")
        {
            ARMARX_CHECK_EQUAL(value.size(), controller.s.numPoints);
            getWriterControlStruct().desiredKeypointPosition = value;
        }
        else if (name == "current")
        {
            ARMARX_CHECK_EQUAL(value.size(), controller.s.numPoints);
            getWriterControlStruct().currentKeypointPosition = value;
        }
        else if (name == "kp")
        {
            ARMARX_CHECK_EQUAL(value.size(), controller.s.numPoints);
            getWriterControlStruct().keypointKp = value;
        }
        else if (name == "kd")
        {
            ARMARX_CHECK_EQUAL(value.size(), controller.s.numPoints);
            getWriterControlStruct().keypointKd = value;
        }
        else if (name == "translation")
        {
            ARMARX_CHECK_EQUAL(value.size(), controller.s.numPoints);
            getWriterControlStruct().fixedTranslation = value;
        }
    }
 
    void
    NJointKeypointsAdmittanceController::setControlParameters(const std::string& name,
                                                              const Eigen::VectorXf& value,
                                                              const Ice::Current&)
    {
        LockGuardType guard{controlDataMutex};
        if (name == "impedance_stiffness")
        {
            ARMARX_CHECK_EQUAL(value.size(), 6);
            getWriterControlStruct().kpImpedance = value;
        }
        else if (name == "impedance_damping")
        {
            ARMARX_CHECK_EQUAL(value.size(), 6);
            getWriterControlStruct().kdImpedance = value;
        }
        else if (name == "admittance_stiffness")
        {
            ARMARX_CHECK_EQUAL(value.size(), 6);
            getWriterControlStruct().kpAdmittance = value;
        }
        else if (name == "admittance_damping")
        {
            ARMARX_CHECK_EQUAL(value.size(), 6);
            getWriterControlStruct().kdAdmittance = value;
        }
        else if (name == "admittance_inertia")
        {
            ARMARX_CHECK_EQUAL(value.size(), 6);
            getWriterControlStruct().kmAdmittance = value;
        }
        else if (name == "nullspace_stiffness")
        {
            ARMARX_CHECK_EQUAL((unsigned)value.size(), targets.size());
            getWriterControlStruct().kpNullspace = value;
        }
        else if (name == "nullspace_damping")
        {
            ARMARX_CHECK_EQUAL((unsigned)value.size(), targets.size());
            getWriterControlStruct().kdNullspace = value;
        }
        else if (name == "desired_nullspace_joint_angles")
        {
            ARMARX_CHECK_EQUAL((unsigned)value.size(), targets.size());
            getWriterControlStruct().desiredNullspaceJointAngles = value;
        }
        else
        {
            ARMARX_ERROR << name << " is not supported by TaskSpaceAdmittanceController";
        }
        writeControlStruct();
    }
 
    void
    NJointKeypointsAdmittanceController::setForceTorqueBaseline(
        const Eigen::Vector3f& forceBaseline,
        const Eigen::Vector3f& torqueBaseline,
        const Ice::Current&)
    {
        ftSensorBuffer.getWriteBuffer().forceBaseline = forceBaseline;
        ftSensorBuffer.getWriteBuffer().torqueBaseline = torqueBaseline;
        ftSensorBuffer.commitWrite();
    }
 
    void
    NJointKeypointsAdmittanceController::setTCPMass(Ice::Float mass, const Ice::Current&)
    {
        ftSensorBuffer.getWriteBuffer().enableTCPGravityCompensation = true;
        ftSensorBuffer.getWriteBuffer().tcpMass = mass;
        ftSensorBuffer.commitWrite();
    }
 
    void
    NJointKeypointsAdmittanceController::setTCPCoMInFTFrame(
        const Eigen::Vector3f& tcpCoMInFTSensorFrame,
        const Ice::Current&)
    {
        ftSensorBuffer.getWriteBuffer().enableTCPGravityCompensation = true;
        ftSensorBuffer.getWriteBuffer().tcpCoMInFTSensorFrame = tcpCoMInFTSensorFrame;
        ftSensorBuffer.commitWrite();
    }
 
    void
    NJointKeypointsAdmittanceController::calibrateFTSensor(const Ice::Current&)
    {
        rtReady.store(false);
    }
 
    void
    NJointKeypointsAdmittanceController::toggleGravityCompensation(const bool toggle,
                                                                   const Ice::Current&)
    {
        ftSensorBuffer.getWriteBuffer().enableTCPGravityCompensation = toggle;
        ftSensorBuffer.commitWrite();
        rtReady.store(false); /// you also need to re-calibrate ft sensor
    }
 
    void
    NJointKeypointsAdmittanceController::rtPreActivateController()
    {
        VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(kinematicChainName);
        Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
        ARMARX_IMPORTANT << "rt preactivate controller with target pose\n\n" << currentPose;
        controller.preactivateInit(rns);
        getWriterControlStruct().desiredNullspaceJointAngles =
            controller.s.desiredNullspaceJointAngles;
        writeControlStruct();
 
        controlStatusBuffer.reinitAllBuffers(controller.s);
    }
} // namespace armarx::control::njoint_controller::task_space