KeypointsImpedanceController.cpp

Go to the documentation of this file.

#include <fstream>
#include <boost/algorithm/clamp.hpp>
 
#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 <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/json.h>
 
#include <VirtualRobot/MathTools.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include "../utils.h"
#include "KeypointsImpedanceController.h"
 
 
 
namespace armarx::control::common::control_law
{
 
    void
    KeypointsImpedanceController::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));
 
        /// joint space variables
        numOfJoints = rns->getSize();
        I = Eigen::MatrixXf::Identity(numOfJoints, numOfJoints);
    }
 
 
    void
    KeypointsImpedanceController::preactivateInit(const VirtualRobot::RobotNodeSetPtr& rns, const Config& c)
    {
        ARMARX_INFO << "preactivate control law KeypointsImpedanceController";
        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.desiredVel.setZero();
            s.desiredAcc.setZero();
 
            s.jacobi.setZero(6, numOfJoints);
            s.jtpinv.setZero(6, numOfJoints);
 
            /// keypoint status
            if (c.isRigid)
            {
                for (int i = 0; i < c.numPoints; i++)
                {
                    s.currentKeypointPosition.segment(3*i, 3) = currentPose.block(0, 3, 3, 1);
                }
                s.currentKeypointPosition = s.currentKeypointPosition + c.fixedTranslation;
            }
            ARMARX_CHECK_EQUAL(s.currentKeypointPosition.size(), c.numPoints * 3);
            s.currentKeypointVelocity.setZero(c.numPoints*3);
 
            s.pointTrackingForce.setZero();
            s.filteredKeypointPosition = s.currentKeypointPosition;
            s.previousKeypointPosition = s.currentKeypointPosition;
 
            s.desiredKeypointPosition  = s.currentKeypointPosition;
            s.desiredKeypointVelocity.setZero(c.numPoints*3);
            s.desiredDensityForce.setZero(c.numPoints*3);
 
            bufferNonRtToRt.reinitAllBuffers(s);
            bufferNonRtToOnPublish.reinitAllBuffers(s);
        }
 
        {
            RtStatus s;
 
            s.desiredPose = currentPose;
            s.desiredTwist.setZero();
 
            s.forceImpedance.setZero();
 
            s.nullspaceTorque.setZero(numOfJoints);
            s.desiredJointTorques.setZero(numOfJoints);
 
            bufferRtToOnPublish.reinitAllBuffers(s);
        }
    }
 
    void
    KeypointsImpedanceController::firstRun()
    {
        auto& sRt = bufferRtToOnPublish.getWriteBuffer();
        auto& sNoneRt = bufferNonRtToRt.getUpToDateReadBuffer();
 
        sRt.desiredPose = sNoneRt.currentPose;
        sRt.desiredTwist.setZero();
 
        sRt.forceImpedance.setZero();
 
        sRt.nullspaceTorque.setZero();
        sRt.desiredJointTorques.setZero();
 
        bufferRtToOnPublish.commitWrite();
    }
 
 
    bool
    KeypointsImpedanceController::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();
        Eigen::MatrixXf jacobi = ik->getJacobianMatrix(tcp, VirtualRobot::IKSolver::CartesianSelection::All);
        jacobi.block(0, 0, 3, numOfJoints) = 0.001 * jacobi.block(0, 0, 3, numOfJoints);
        ARMARX_CHECK_EQUAL(numOfJoints, jacobi.cols());
        ARMARX_CHECK_EQUAL(6, jacobi.rows());
        s.jacobi = jacobi;
 
        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 = jacobi * s.qvel;
 
        /// ----------------------------- keypoint related ---------------------------------------------
        /// compute (filtered) keypoint position
        if (cfg.isRigid)
        {
            for (int i = 0; i < cfg.numPoints; i++)
            {
                s.currentKeypointPosition.segment(3*i, 3) = s.currentPose.block(0, 0, 3, 3) * cfg.fixedTranslation.segment(3*i, 3) + s.currentPose.block<3, 1>(0, 3);
            }
        }
        ARMARX_CHECK_EQUAL(cfg.numPoints * 3, s.currentKeypointPosition.size());
        s.filteredKeypointPosition  = (1.0f - cfg.keypointPositionFilter) * s.filteredKeypointPosition + cfg.keypointPositionFilter * s.currentKeypointPosition;
        ARMARX_CHECK_EQUAL(cfg.numPoints * 3, s.filteredKeypointPosition.size());
        /// compute filtered keypoint velocity
        if (cfg.isRigid)
        {
            Eigen::VectorXf currentKeypointVelocity;
            currentKeypointVelocity.setZero(cfg.numPoints * 3);
            for (int i = 0; i < cfg.numPoints; i++)
            {
                Eigen::Vector3f angular_vel = s.currentTwist.tail(3);
                Eigen::Vector3f dist = cfg.fixedTranslation.segment(3*i, 3);
                currentKeypointVelocity.segment(3*i, 3) = angular_vel.cross(dist) + s.currentTwist.head(3);
            }
            s.currentKeypointVelocity = (1.0f - cfg.keypointVelocityFilter) * s.currentKeypointVelocity
                    + cfg.keypointVelocityFilter * currentKeypointVelocity;
        }
        else
        {
            s.currentKeypointVelocity = (1.0f - cfg.keypointVelocityFilter) * s.currentKeypointVelocity
                    + cfg.keypointVelocityFilter * (s.filteredKeypointPosition - s.previousKeypointPosition) / static_cast<float>(robotStatus.deltaT);
        }
        s.previousKeypointPosition = s.filteredKeypointPosition;
 
        /// ----------------------------- compute keypoint tracking force ---------------------------------------------
        auto difference = s.desiredKeypointPosition - s.filteredKeypointPosition;
        /// in theory, we can also add s.desiredKeypointVelocity to the damping term
        Eigen::VectorXf trackingForce = difference.cwiseProduct(cfg.keypointKp) - s.currentKeypointVelocity.cwiseProduct(cfg.keypointKd);
        if (trackingForce.size() != 3 * cfg.numPoints)
        {
            trackingForce.setZero(3 * cfg.numPoints);
        }
 
        Eigen::Vector3f meanKeypointPosition = Eigen::Vector3f::Zero();
        for (int i = 0; i < cfg.numPoints; i++)
        {
            meanKeypointPosition = meanKeypointPosition + s.filteredKeypointPosition.segment(3*i, 3);
        }
        meanKeypointPosition = meanKeypointPosition / cfg.numPoints;
 
        s.pointTrackingForce.setZero();
        for (int i = 0; i < cfg.numPoints; i++)
        {
            //        Eigen::Vector3f dist = s.filteredKeypointPosition.segment(3*i, 3) - s.currentPose.block<3, 1>(0, 3);
            Eigen::Vector3f dist = s.filteredKeypointPosition.segment(3*i, 3) - meanKeypointPosition;
            dist = dist * 0.001;
            Eigen::Vector3f force = trackingForce.segment(3*i, 3) + s.desiredDensityForce.segment(3*i, 3);
            if (force.norm() > 1000)
            {
                ARMARX_RT_LOGF_WARN("force too large, set to zero");
                force.setZero();
            }
            s.pointTrackingForce.head(3) += force;
            s.pointTrackingForce.tail(3) += dist.cross(force);
        }
        Eigen::Vector6f acc =
                cfg.kmAdmittance.cwiseProduct(s.pointTrackingForce) -
                cfg.kdAdmittance.cwiseProduct(s.desiredVel);
 
        Eigen::Vector6f vel = s.desiredVel + 0.5 * static_cast<float>(robotStatus.deltaT) * (acc + s.desiredAcc);
        Eigen::VectorXf deltaPose = 0.5 * static_cast<float>(robotStatus.deltaT) * (vel + s.desiredVel);
        s.desiredAcc = acc;
        s.desiredVel = vel;
 
        Eigen::Matrix3f deltaPoseMat = VirtualRobot::MathTools::rpy2eigen3f(deltaPose(3), deltaPose(4), deltaPose(5));
        s.desiredPose.block<3, 1>(0, 3) += deltaPose.head(3);
        s.desiredPose.block<3, 3>(0, 0) = deltaPoseMat * s.desiredPose.block<3, 3>(0, 0);
 
        bufferNonRtToOnPublish.getWriteBuffer() = s;
 
        bufferNonRtToOnPublish.commitWrite();
        bufferNonRtToRt.commitWrite();
 
        isInitialized.store(true);
        return true;
    }
 
    void
    KeypointsImpedanceController::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 \n\n %s\n\nis too far away from\n\n %s", VAROUT(s.desiredPose), VAROUT(sRt.desiredPose));
            }
            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