FTSensor.cpp

Go to the documentation of this file.

#include "FTSensor.h"
 
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Nodes/Sensor.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
 
#include "../utils.h"
 
namespace armarx::control::common::ft
{
 
    void
    FTSensor::initialize(const VirtualRobot::RobotNodeSetPtr& rns,
                         const RobotUnitPtr& robotUnit,
                         const FTConfig& c)
    {
        ARMARX_INFO << "--> Initializing force torque sensor " << c.sensorName;
        const SensorValueBase* svlf = robotUnit->getSensorDevice(c.sensorName)->getSensorValue();
        forceSensor = svlf->asA<SensorValueForceTorque>();
 
        ARMARX_CHECK_NOT_NULL(rns);
        sensorNode = rns->getRobot()->getSensor(c.sensorName)->getRobotNode();
 
        if (c.sensorName == "FT R")
        {
            tcpMass = calibDefaults.tcpMassRight;
            tcpCoMInFTSensorFrame = calibDefaults.tcpCoMInFTFrameRight;
        }
        else if (c.sensorName == "FT L")
        {
            tcpMass = calibDefaults.tcpMassLeft;
            tcpCoMInFTSensorFrame = calibDefaults.tcpCoMInFTFrameLeft;
        }
 
        rawForce.setZero();
        rawTorque.setZero();
        filteredForce.setZero();
        filteredTorque.setZero();
        filteredForceInRoot.setZero();
        filteredTorqueInRoot.setZero();
 
        tcpGravityCompensation.setZero();
        tcpGravForceVec.setZero();
        tcpGravTorqueVec.setZero();
 
        sensorOriInRoot = Eigen::Matrix3f::Identity();
        reset();
        ARMARX_INFO << "<-- Force torque sensor initialized.";
    }
 
    //FTSensor::FTConfig FTSensor::reconfigure(const std::string &configFileName)
    //{
    //    nlohmann::json userConfig;
    //    std::ifstream ifs{configFileName};
    //    ifs >> userConfig;
    //    auto c = userConfig["force_torque"];
    //    FTConfig config = FTConfig();
    //    std::string name       = getValueWithDefault<std::string>(c, "", "sensor_name");
    //    if (name != sensorName)
    //    {
    //        ARMARX_ERROR << "the sensor name: " << name << " is not the same as initialized: " << sensorName << ", please consider to switch controllers instead of reuse";
    //    }
    //    config.ftFilter        = getValueWithDefault<float>(c, s.ftFilter, "force_filter_coeff");
    //    config.deadZoneForce   = getValueWithDefault<float>(c, s.deadZoneForce, "force_dead_zone");
    //    config.deadZoneTorque  = getValueWithDefault<float>(c, s.deadZoneTorque, "torque_dead_zone");
    //    config.timeLimit       = getValueWithDefault<float>(c, s.timeLimit, "wait_time_for_calibration");
    //    s.timeLimit = config.timeLimit;
 
    //    config.enableTCPGravityCompensation = getValueWithDefault<bool>(c, s.enableTCPGravityCompensation, "enable_tcp_gravity_compensation");
    //    nlohmann::json compCfg;
    //    if (c.find(sensorName) != c.end())
    //        auto compCfg = c[sensorName];
    //    config.tcpMass = getValueWithDefault<float>(compCfg, s.tcpMass, "tcp_mass");
    //    config.tcpCoMInFTSensorFrame = getEigenVecWithDefault(compCfg, s.tcpCoMInFTSensorFrame, "tcp_CoM_in_FT_frame");
    //    config.forceBaseline.setZero();
    //    config.torqueBaseline.setZero();
    //    return config;
    //}
 
    void
    FTSensor::reset()
    {
        /// you have to call this method **only** in the first loop of rtRun
        //    s.tcpMass = 0.0f;
        //    s.tcpCoMInFTSensorFrame.setZero();
 
        forceOffset.setZero();
        torqueOffset.setZero();
        ft.setZero();
 
        calibrated.store(false);
        timeForCalibration = 0.0f;
 
        safeGuardForceOffset.setZero();
        safeGuardTorqueOffset.setZero();
 
        safeGuardEnabledForce.store(false);
        safeGuardEnabledTorque.store(false);
 
        resetForceGuard.store(true);
        resetTorqueGuard.store(true);
 
        ftWasNotSafe.store(false);
 
        //    Eigen::Matrix4f forceFrameInRoot = sensorNode->getPoseInRootFrame();
        //    forceFrameInTCP.block<3, 3>(0, 0) = currentPose.block<3, 3>(0, 0).transpose() * forceFrameInRoot.block<3, 3>(0, 0);
        //    tcpCoMInTCPFrame = forceFrameInTCP.block<3, 3>(0, 0) * tcpCoMInFTSensorFrame;
    }
 
    void
    FTSensor::updateStatus(const FTConfig& c,
                           Eigen::Vector6f& currentFT,
                           double deltaT,
                           bool firstRun)
    {
        if (firstRun)
        {
            reset();
            return;
        }
        sensorOriInRoot = sensorNode->getOrientationInRootFrame();
        rawForce = forceSensor->force;
        rawTorque = forceSensor->torque;
 
        if (c.reCalibrate)
        {
            calibrated.store(false);
            /// TODO how to toggle recalibration?
        }
        compensateTCPGravity(c);
        if (!calibrated.load())
        {
            if (calibrate(c, deltaT))
            {
                calibrated.store(true);
            }
        }
        else
        {
            currentFT = getFilteredForceTorque(c);
        }
        resetSafeGuardOffset(c);
    }
 
    void
    FTSensor::resetSafeGuardOffset(const FTConfig& c)
    {
        if (c.enableSafeGuardForce and resetForceGuard.load())
        {
            safeGuardForceOffset = filteredForceInRoot;
            ARMARX_INFO << "Resetting force safe guard with baseline "
                        << VAROUT(filteredForceInRoot);
            ftWasNotSafe.store(false);
            safeGuardEnabledForce.store(true);
            resetForceGuard.store(false);
        }
        if (c.enableSafeGuardTorque and resetTorqueGuard.load())
        {
            safeGuardTorqueOffset = filteredTorqueInRoot;
            ftWasNotSafe.store(false);
            safeGuardEnabledTorque.store(true);
            resetTorqueGuard.store(false);
        }
    }
 
    void
    FTSensor::enableSafeGuard(bool enableForceGuard, bool enableTorqueGuard)
    {
        if (enableForceGuard)
        {
            resetForceGuard.store(true);
        }
        if (enableTorqueGuard)
        {
            resetTorqueGuard.store(true);
        }
    }
 
    bool
    FTSensor::isForceGuardEnabled()
    {
        return safeGuardEnabledForce.load();
    }
 
    bool
    FTSensor::isTorqueGuardEnabled()
    {
        return safeGuardEnabledTorque.load();
    }
 
    bool
    FTSensor::isSafe(const FTConfig& c)
    {
        auto unsafeFlag =
            (c.enableSafeGuardForce and
             (filteredForceInRoot - safeGuardForceOffset).norm() > c.safeGuardForceThreshold) or
            (c.enableSafeGuardTorque and
             (filteredTorqueInRoot - safeGuardTorqueOffset).norm() > c.safeGuardTorqueThreshold);
        ftSafe.store(not unsafeFlag);
        if (unsafeFlag)
        {
            ftWasNotSafe.store(true);
        }
 
        return ftSafe.load();
    }
 
    bool
    FTSensor::calibrate(const FTConfig& c, double deltaT)
    {
        /// this will calibrate the force torque value in FTSensorFrame until timeLimit is reached
        //        sensorOriInRoot = sensorNode->getOrientationInRootFrame();
        forceOffset = (1 - c.ftFilter) * forceOffset +
                      c.ftFilter * (forceSensor->force -
                                    sensorOriInRoot.transpose() * tcpGravityCompensation.head<3>());
        torqueOffset = (1 - c.ftFilter) * torqueOffset +
                       c.ftFilter * (forceSensor->torque - sensorOriInRoot.transpose() *
                                                               tcpGravityCompensation.tail<3>());
 
        timeForCalibration = timeForCalibration + deltaT;
        if (timeForCalibration > c.timeLimit)
        {
            calibrated.store(true);
            /// rt is ready only if the FT sensor is calibrated, and the rt2interface buffer is updated (see above)
            ARMARX_RT_LOGF_IMPORTANT(
                "FT calibration done \n"
                "-- force offset: [%.2f, %.2f, %.2f] \n"
                "-- torque offset: [%.2f, %.2f, %.2f] \n"
                "-- gravity compensation: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f]",
                forceOffset(0),
                forceOffset(1),
                forceOffset(2),
                torqueOffset(0),
                torqueOffset(1),
                torqueOffset(2),
                tcpGravityCompensation(0),
                tcpGravityCompensation(1),
                tcpGravityCompensation(2),
                tcpGravityCompensation(3),
                tcpGravityCompensation(4),
                tcpGravityCompensation(5));
        }
        return calibrated.load();
    }
 
    //void FTSensor::enableGravityCompensation(const float mass, const Eigen::Vector3f& tcpCoMInFTFrame)
    //{
    //    enableTCPGravityCompensation.store(true);
    //    tcpMass = mass;
    //    tcpCoMInFTSensorFrame = tcpCoMInFTFrame;
    //}
 
    //void FTSensor::disableGravityCompensation()
    //{
    //    enableTCPGravityCompensation.store(false);
    //    tcpMass = 0.0f;
    //    tcpCoMInFTSensorFrame.setZero();
    //    tcpGravityCompensation.setZero();
    //}
 
    Eigen::Vector6f&
    FTSensor::compensateTCPGravity(const FTConfig& c)
    {
        //    /// update variables from rt buffer
        //    tcpMass = data.tcpMass;
        //    tcpCoMInTCPFrame = forceFrameInTCP.block<3, 3>(0, 0) * data.tcpCoMInFTSensorFrame;
 
        /// static compensation in root frame
        if (c.enableTCPGravityCompensation)
        {
 
            tcpGravForceVec = tcpMass * gravity.head<3>();
            tcpGravTorqueVec = (sensorOriInRoot * tcpCoMInFTSensorFrame).cross(tcpGravForceVec);
            //            tcpGravityCompensation << forceVec, torqueVec;
            //        Eigen::Vector3f localGravity = currentPose.block<3, 3>(0, 0).transpose() * gravity;
            //        Eigen::Vector3f localForceVec = tcpMass * localGravity;
            //        Eigen::Vector3f localTorqueVec = tcpCoMInTCPFrame.cross(localForceVec);
 
            //        tcpGravityCompensation << localForceVec, localTorqueVec;
            tcpGravityCompensation.head<3>() = tcpGravForceVec;
            tcpGravityCompensation.tail<3>() = tcpGravTorqueVec;
        }
        else
        {
            tcpGravityCompensation.setZero();
        }
        return tcpGravityCompensation;
    }
 
    Eigen::Vector6f&
    FTSensor::getFilteredForceTorque(const FTConfig& c)
    {
        filteredForce = (1 - c.ftFilter) * filteredForce + c.ftFilter * rawForce;
        filteredTorque = (1 - c.ftFilter) * filteredTorque + c.ftFilter * rawTorque;
 
        filteredForceInRoot = sensorOriInRoot * (filteredForce - forceOffset) -
                              tcpGravityCompensation.head<3>() - c.forceBaseline;
        filteredTorqueInRoot = sensorOriInRoot * (filteredTorque - torqueOffset) -
                               tcpGravityCompensation.tail<3>() - c.torqueBaseline;
 
        for (size_t i = 0; i < 3; ++i)
        {
            if (fabs(filteredForceInRoot(i)) > c.deadZoneForce)
            {
                filteredForceInRoot(i) -=
                    (filteredForceInRoot(i) / fabs(filteredForceInRoot(i))) * c.deadZoneForce;
            }
            else
            {
                filteredForceInRoot(i) = 0;
            }
 
            if (fabs(filteredTorqueInRoot(i)) > c.deadZoneTorque)
            {
                filteredTorqueInRoot(i) -=
                    (filteredTorqueInRoot(i) / fabs(filteredTorqueInRoot(i))) * c.deadZoneTorque;
            }
            else
            {
                filteredTorqueInRoot(i) = 0;
            }
        }
 
        ft.head<3>() = filteredForceInRoot;
        ft.tail<3>() = filteredTorqueInRoot;
        return ft;
    }
 
    Eigen::Vector3f&
    FTSensor::getSafeGuardForceOffset()
    {
        return safeGuardForceOffset;
    }
} // namespace armarx::control::common::ft