TaskspaceAdmittanceController.cpp

Go to the documentation of this file.

#include "TaskspaceAdmittanceController.h"
 
#include <fstream>
#include <boost/algorithm/clamp.hpp>
 
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/MathTools.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
 
#include "../utils.h"
 
 
namespace armarx::control::common::control_law
{
 
void TaskspaceAdmittanceController::initialize(const VirtualRobot::RobotNodeSetPtr& rns)
    {
        ARMARX_CHECK_NOT_NULL(rns);
        tcp = rns->getTCP();
        ARMARX_CHECK_NOT_NULL(tcp);
        ik.reset(new VirtualRobot::DifferentialIK(
            rns, rns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
 
        numOfJoints = rns->getSize();
        I = Eigen::MatrixXf::Identity(numOfJoints, numOfJoints);
        //    s.qpos.resize(numOfJoints);
        //    s.qvel.resize(numOfJoints);
        //    s.desiredJointTorques.resize(numOfJoints);
 
        //    s.currentTwist.setZero();
        //    s.forceImpedance.setZero();
        //    s.virtualVel.setZero();
        //    s.virtualAcc.setZero();
 
        //    /// parse configuration files
        //    nlohmann::json userConfig;
        //    nlohmann::json defaultConfig;
        //    std::string filename;
        //    {
        //        ArmarXDataPath::getAbsolutePath(
        //                    "armarx_control/controller_config/default/njoint_controller/impedance_controller_config.json",
        //                    filename);
        //        ARMARX_INFO << "ctrl law: reading default config from: " << VAROUT(filename);
        //        std::ifstream ifs{filename};
        //        ifs >> defaultConfig;
        //        if (defaultConfig.empty())
        //        {
        //            ARMARX_ERROR << "ctrl law: default config is empty!";
        //        }
        //    }
        //    if (configFileName.empty())
        //    {
        //        ARMARX_IMPORTANT << "ctrl law: No control parameter specified by user, use default parameter in \n" << filename;
        //        userConfig = defaultConfig;
        //    }
        //    else
        //    {
        //        std::ifstream ifs{configFileName};
        //        ifs >> userConfig;
        //    }
        //    if (userConfig.empty())
        //    {
        //        ARMARX_ERROR << "ctrl law: userConfig is empty!";
        //    }
 
        //    if (userConfig.find("control") != userConfig.end())
        //    {
        //        auto c = userConfig["control"];
        //        auto dc = defaultConfig["control"];
        //        s.kpImpedance = getEigenVec(c, dc, "impedance_stiffness");
        //        s.kdImpedance = getEigenVec(c, dc, "impedance_damping");
        //        s.kpAdmittance = getEigenVec(c, dc, "admittance_stiffness");
        //        s.kdAdmittance = getEigenVec(c, dc, "admittance_damping");
        //        s.kmAdmittance = getEigenVec(c, dc, "admittance_inertia");
        //        s.kpNullspace = getEigenVec(c, dc, "nullspace_stiffness", numOfJoints, 10.0f);
        //        s.kdNullspace = getEigenVec(c, dc, "nullspace_damping", numOfJoints, 2.0f);
        //        s.desiredNullspaceJointAngles = getEigenVec(c, dc, "desired_nullspace_joint_angles");
        //        if (s.desiredNullspaceJointAngles.size() != numOfJoints)
        //        {
        //            ARMARX_IMPORTANT << "ctrl law: default value is not consistent with requested kinematic chain, reinit in preactivation";
        //            enablePreactivateInit.store(true);
        //        }
        //        s.torqueLimit = getValue<float>(c, dc, "torque_limit");
        //        s.qvelFilter = getValue<float>(c, dc, "qvel_filter");
        //    }
        //    else
        //    {
        //        ARMARX_WARNING << "ctrl law: controller parameters not found, they should be placed in 'control' tag in your json file";
        //    }
 
    }
 
 
    //TaskspaceAdmittanceController::Config TaskspaceAdmittanceController::reconfigure(const std::string &configFileName)
    //{
    //    nlohmann::json userConfig;
    //    std::ifstream ifs{configFileName};
    //    ifs >> userConfig;
    //    auto c = userConfig["control"];
    //    Config config = Config();
    //    config.kpImpedance                  = getEigenVecWithDefault    (c, s.kpImpedance, "impedance_stiffness");
    //    config.kdImpedance                  = getEigenVecWithDefault    (c, s.kdImpedance, "impedance_damping");
    //    config.kpAdmittance                 = getEigenVecWithDefault    (c, s.kpAdmittance, "admittance_stiffness");
    //    config.kdAdmittance                 = getEigenVecWithDefault    (c, s.kdAdmittance, "admittance_damping");
    //    config.kmAdmittance                 = getEigenVecWithDefault    (c, s.kmAdmittance, "admittance_inertia");
    //    config.kpNullspace                  = getEigenVecWithDefault    (c, s.kpNullspace, "nullspace_stiffness");
    //    config.kdNullspace                  = getEigenVecWithDefault    (c, s.kdNullspace, "nullspace_damping");
 
    //    config.currentForceTorque.setZero();
    //    config.desiredPose = s.currentPose;
    //    config.desiredTwist.setZero();
    //    config.desiredNullspaceJointAngles  = getEigenVecWithDefault    (c, s.desiredNullspaceJointAngles, "desired_nullspace_joint_angles");
    //    config.torqueLimit                  = getValueWithDefault<float>(c, s.torqueLimit, "torque_limit");
    //    config.qvelFilter                   = getValueWithDefault<float>(c, s.qvelFilter, "qvel_filter");
    //    return config;
    //}
 
    void TaskspaceAdmittanceController::preactivateInit(const VirtualRobot::RobotNodeSetPtr& rns)
    {
        ARMARX_INFO << "preactivate control law TaskspaceImpedanceController";
        Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
        {
            NonRtStatus s;
 
            s.qpos = rns->getJointValuesEigen();
            s.qvel.setZero(numOfJoints);
 
            s.currentPose = currentPose;
            s.currentTwist.setZero();
 
            s.jacobi.setZero(6, numOfJoints);
            s.jtpinv.setZero(6, numOfJoints);
 
            bufferNonRtToRt.reinitAllBuffers(s);
            bufferNonRtToOnPublish.reinitAllBuffers(s);
        }
 
        {
            RtStatus s;
            s.desiredJointTorques.setZero(numOfJoints);
            s.nullspaceTorque.setZero(numOfJoints);
 
            s.virtualPose = currentPose;
            s.virtualVel.setZero();
            s.virtualAcc.setZero();
 
            s.desiredPose = currentPose;
            s.desiredTwist.setZero();
            s.kmAdmittance.setZero();
 
            s.currentForceTorque.setZero();
 
            s.forceImpedance.setZero();
            bufferRtToOnPublish.reinitAllBuffers(s);
        }
        ARMARX_INFO << "preactivation done";
    }
 
    void TaskspaceAdmittanceController::firstRun()
    {
        auto& sRt = bufferRtToOnPublish.getWriteBuffer();
        sRt.desiredPose = bufferNonRtToRt.getUpToDateReadBuffer().currentPose;
        sRt.currentForceTorque.setZero();
 
        bufferRtToOnPublish.commitWrite();
        ftsensor.reset();
//        isCalibrated.store(false);
        //    s.previousDesiredPose = s.currentPose;
        //    s.desiredPose = s.currentPose;
        //    s.desiredTwist.setZero();
        //    s.virtualPose = s.currentPose;
        //    s.virtualVel.setZero();
        //    s.virtualAcc.setZero();
        //    ARMARX_INFO << VAROUT(s.desiredTCPPose) << VAROUT(s.previousTargetPose);
    }
 
 
    bool
    TaskspaceAdmittanceController::updateControlStatus(
            const Config& cfg,
            const RobotStatus& robotStatus)
    {
        auto& s = bufferNonRtToRt.getWriteBuffer();
 
        /// check size and value
        {
            ARMARX_CHECK_EQUAL((unsigned)numOfJoints, robotStatus.jointPosition.size());
            ARMARX_CHECK_EQUAL((unsigned)numOfJoints, robotStatus.jointVelocity.size());
            ARMARX_CHECK_EQUAL((unsigned)numOfJoints, robotStatus.jointTorque.size());
        }
 
        /// ----------------------------- get current status of robot ---------------------------------------------
        /// original data in ArmarX use the following units:
        /// position in mm,
        /// joint angles and orientation in radian,
        /// velocity in mm/s and radian/s, etc.
        /// here we convert mm to m, if you use MP from outside, make sure to convert it back to mm
 
        s.currentPose = tcp->getPoseInRootFrame();
        s.jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
        s.jacobi.block(0, 0, 3, numOfJoints) = 0.001 * s.jacobi.block(0, 0, 3, numOfJoints);
        ARMARX_CHECK_EQUAL(numOfJoints, s.jacobi.cols());
        ARMARX_CHECK_EQUAL(6, s.jacobi.rows());
 
        s.jtpinv = ik->computePseudoInverseJacobianMatrix(s.jacobi.transpose(), lambda);
        ARMARX_CHECK_EQUAL(numOfJoints, s.jtpinv.cols());
        ARMARX_CHECK_EQUAL(6, s.jtpinv.rows());
 
        Eigen::VectorXf qvelRaw(numOfJoints);
        for (size_t i = 0; i < robotStatus.jointVelocity.size(); ++i)
        {
            s.qpos(i) = robotStatus.jointPosition[i];
            qvelRaw(i) = robotStatus.jointVelocity[i];
        }
        ARMARX_CHECK_EQUAL(s.qvel.rows(), qvelRaw.rows());
 
        s.qvel = (1 - cfg.qvelFilter) * s.qvel + cfg.qvelFilter * qvelRaw;
        s.currentTwist = s.jacobi * s.qvel;
 
        bufferNonRtToOnPublish.getWriteBuffer() = s;
 
        bufferNonRtToOnPublish.commitWrite();
        bufferNonRtToRt.commitWrite();
 
        isInitialized.store(true);
        return true;
    }
 
    void
    TaskspaceAdmittanceController::updateFT(const common::ft::FTSensor::FTConfig& c, double deltaT)
    {
        /// run in rt thread
 
        if (c.reCalibrate)
        {
            ftsensor.calibrated.store(false);
            /// TODO how to toggle recalibration?
        }
        ftsensor.compensateTCPGravity(c);
        if (!ftsensor.calibrated.load())
        {
            if(ftsensor.calibrate(c, deltaT))
            {
                ftsensor.calibrated.store(true);
            }
        }
        else
        {
            bufferRtToOnPublish.getWriteBuffer().currentForceTorque = ftsensor.getFilteredForceTorque(c);
            bufferRtToOnPublish.commitWrite();
        }
    }
 
    void
    TaskspaceAdmittanceController::run(
            const Config& c,
            ControlTarget& targets,
            double deltaT)
    {
        /// run in rt thread
 
        auto& sRt = bufferRtToOnPublish.getWriteBuffer();
        const auto& s = bufferNonRtToRt.getUpToDateReadBuffer();
 
        /// only when the ft sensor is calibrated can we allow the admittance controller
        /// to have virtual pose modulation out of force/torque information.
        if (not ftsensor.calibrated.load() or not isInitialized.load())
        {
            sRt.desiredTwist.setZero();
            sRt.kmAdmittance.setZero();
            sRt.currentForceTorque.setZero();
        }
        else
        {
            if ((sRt.desiredPose.block<3, 1>(0, 3) - c.desiredPose.block<3, 1>(0, 3)).norm() > 100.0f)
            {
                sRt.currentForceTorque.setZero();
                ARMARX_RT_LOGF_WARN("new target \n\n %s\n\nis too far away from\n\n %s", VAROUT(c.desiredPose), VAROUT(sRt.desiredPose));
            }
            else
            {
                sRt.desiredPose = c.desiredPose;
            }
            sRt.desiredTwist = c.desiredTwist;
            sRt.kmAdmittance = c.kmAdmittance;
        }
        /// ---------------------------- admittance control ---------------------------------------------
        /// calculate pose error between the virtual pose and the target pose
        Eigen::Matrix3f objDiffMat = sRt.virtualPose.block<3, 3>(0, 0) * sRt.desiredPose.block<3, 3>(0, 0).transpose();
        Eigen::Vector6f poseError;
        poseError.head(3) = sRt.virtualPose.block<3, 1>(0, 3) - sRt.desiredPose.block<3, 1>(0, 3);
        poseError.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
 
        /// admittance control law and Euler Integration -> virtual pose
        Eigen::Vector6f acc =
                sRt.kmAdmittance.cwiseProduct(sRt.currentForceTorque) -
                c.kpAdmittance.cwiseProduct(poseError) -
                c.kdAdmittance.cwiseProduct(sRt.virtualVel);
 
        Eigen::Vector6f vel = sRt.virtualVel + 0.5 * deltaT * (acc + sRt.virtualAcc);
        Eigen::Vector6f deltaPose = 0.5 * deltaT * (vel + sRt.virtualVel);
        sRt.virtualAcc = acc;
        sRt.virtualVel = vel;
 
        Eigen::Matrix3f deltaPoseMat = VirtualRobot::MathTools::rpy2eigen3f(deltaPose(3), deltaPose(4), deltaPose(5));
        sRt.virtualPose.block<3, 1>(0, 3) += deltaPose.head(3);
        sRt.virtualPose.block<3, 3>(0, 0) = deltaPoseMat * sRt.virtualPose.block<3, 3>(0, 0);
 
        /// ----------------------------- Impedance control ---------------------------------------------
        /// calculate pose error between target pose and current pose
        /// !!! This is very important: you have to keep postion and orientation both
        ///     with UI unit (meter, radian) to calculate impedance force.
 
        Eigen::Matrix3f diffMat =
                sRt.virtualPose.block<3, 3>(0, 0) * s.currentPose.block<3, 3>(0, 0).transpose();
        Eigen::Vector6f poseErrorImp;
        poseErrorImp.head(3) = 0.001 * (sRt.virtualPose.block<3, 1>(0, 3) - s.currentPose.block<3, 1>(0, 3));
        poseErrorImp.tail(3) = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
        sRt.forceImpedance =
                c.kpImpedance.cwiseProduct(poseErrorImp) -
                c.kdImpedance.cwiseProduct(s.currentTwist);
 
        /// ----------------------------- Nullspace PD Control --------------------------------------------------
        ARMARX_CHECK_EQUAL((unsigned)numOfJoints, c.kpNullspace.rows());
        ARMARX_CHECK_EQUAL((unsigned)numOfJoints, s.qpos.rows());
        ARMARX_CHECK_EQUAL((unsigned)numOfJoints, s.qvel.rows());
 
        Eigen::VectorXf nullspaceTorque =
                c.kpNullspace.cwiseProduct(c.desiredNullspaceJointAngles.value() - s.qpos) -
                c.kdNullspace.cwiseProduct(s.qvel);
 
        /// ----------------------------- Map TS target force to JS --------------------------------------------------
        ARMARX_CHECK_EQUAL((unsigned)6, s.jacobi.rows());
        ARMARX_CHECK_EQUAL((unsigned)numOfJoints, s.jacobi.cols());
 
        ARMARX_CHECK_EQUAL((unsigned)6, s.jtpinv.rows());
        ARMARX_CHECK_EQUAL((unsigned)numOfJoints, s.jtpinv.cols());
 
        sRt.desiredJointTorques =
                s.jacobi.transpose() * sRt.forceImpedance +
                (I - s.jacobi.transpose() * s.jtpinv) * nullspaceTorque;
 
        /// ----------------------------- write torque target  --------------------------------------------------
        for (size_t i = 0; i < (unsigned)sRt.desiredJointTorques.rows(); ++i)
        {
            sRt.desiredJointTorques(i) = std::clamp(sRt.desiredJointTorques(i), -c.torqueLimit, c.torqueLimit);
            targets.targets.at(i) = sRt.desiredJointTorques(i);
        }
 
        bufferRtToOnPublish.commitWrite();
    }
} /// namespace armarx::control::common::control_law