TaskspaceImpedanceController.cpp

Go to the documentation of this file.

#include "TaskspaceImpedanceController.h"
 
#include <fstream>
 
#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 <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
 
#include "../utils.h"
 
 
namespace armarx::control::common::control_law
{
 
    void
    TaskspaceImpedanceController::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);
    }
 
 
    void
    TaskspaceImpedanceController::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.desiredPose = currentPose;
            s.desiredTwist.setZero();
 
            s.jacobi.setZero(6, numOfJoints);
            s.jtpinv.setZero(6, numOfJoints);
 
            bufferNonRtToRt.reinitAllBuffers(s);
            bufferNonRtToOnPublish.reinitAllBuffers(s);
            bufferNonRtToUser.reinitAllBuffers(s);
        }
 
        {
            RtStatus s;
            s.desiredJointTorques.setZero(numOfJoints);
            s.desiredPose = currentPose;
            s.forceImpedance.setZero();
            bufferRtToOnPublish.reinitAllBuffers(s);
        }
        ARMARX_INFO << "preactivation done";
    }
 
     void TaskspaceImpedanceController::firstRun()
     {
         auto& sRt = bufferRtToOnPublish.getWriteBuffer();
         auto& sNoneRt = bufferNonRtToRt.getUpToDateReadBuffer();
         sRt.desiredPose = sNoneRt.currentPose;
         sRt.forceImpedance.setZero();
         sRt.desiredJointTorques.setZero();
         bufferRtToOnPublish.commitWrite();
     }
 
 
    bool
    TaskspaceImpedanceController::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;
 
        s.desiredPose = cfg.desiredPose;
        s.desiredTwist = cfg.desiredTwist;
 
        bufferNonRtToOnPublish.getWriteBuffer() = s;
        bufferNonRtToOnPublish.commitWrite();
        bufferNonRtToUser.getWriteBuffer() = s;
        bufferNonRtToUser.commitWrite();
        bufferNonRtToRt.commitWrite();
 
        isInitialized.store(true);
        return true;
    }
 
    void
    TaskspaceImpedanceController::run(
            const Config& c,
            ControlTarget& targets)
    {
        /// run in rt thread
 
        auto& sRt = bufferRtToOnPublish.getWriteBuffer();
        const auto& s = bufferNonRtToRt.getUpToDateReadBuffer();
 
        if (isInitialized.load())
        {
            if ((sRt.desiredPose.block<3, 1>(0, 3) - s.desiredPose.block<3, 1>(0, 3)).norm() > 100.0f)
            {
                ARMARX_RT_LOGF_WARN("new target is too far away from the current target");
//                ARMARX_RT_LOGF_WARN("new target \n\n [%f, %f, %f]\n\nis too far away from the current target\n\n [%f, %, %s]",
//                                    c.desiredPose(0, 3), c.desiredPose(1, 3), c.desiredPose(2, 3),
//                                    sRt.desiredPose(0, 3), sRt.desiredPose(1, 3), sRt.desiredPose(2, 3));
            }
            else
            {
                sRt.desiredPose = s.desiredPose;
            }
        }
 
        /// ----------------------------- 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.desiredPose.block<3, 3>(0, 0) * s.currentPose.block<3, 3>(0, 0).transpose();
        Eigen::Vector6f poseErrorImp;
        poseErrorImp.head(3) = 0.001 * (sRt.desiredPose.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