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 <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <VirtualRobot/MathTools.h>
 
#include "KeypointsAdmittanceController.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