TaskspaceAdmittanceController.cpp

Go to the documentation of this file.

#include "TaskspaceAdmittanceController.h"
 
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/IK/JacobiProvider.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
 
#include <armarx/control/common/device.h>
 
#include "../utils.h"
 
namespace armarx::control::common::control_law
{
 
    void
    TaskspaceAdmittanceController::initialize(const VirtualRobot::RobotNodeSetPtr& rns,
                                              const VirtualRobot::RobotNodeSetPtr& rtRns)
    {
        ARMARX_CHECK_NOT_NULL(rns);
        tcp = rns->getTCP();
        rtTCP = rtRns->getTCP();
        ARMARX_CHECK_NOT_NULL(tcp);
        ik.reset(new VirtualRobot::DifferentialIK(
            rtRns, rtRns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
 
        // auto numOfJoints = rns->getSize();
        // I = Eigen::MatrixXf::Identity(numOfJoints, numOfJoints);
    }
 
    void
    TaskspaceAdmittanceController::preactivateInit(const VirtualRobot::RobotNodeSetPtr& rns)
    {
    }
 
    void
    TaskspaceAdmittanceController::firstRun()
    {
        ftsensor.reset();
    }
 
    // void
    // TaskspaceAdmittanceController::updateFT(const FTConfig& c, RtStatus& rtStatus)
    // {
    //     /// run in rt thread
    //     ftsensor.updateStatus();
    //     if (c.reCalibrate)
    //     {
    //         ftsensor.calibrated.store(false);
    //         /// TODO how to toggle recalibration?
    //     }
    //     ftsensor.compensateTCPGravity(c);
    //     if (!ftsensor.calibrated.load())
    //     {
    //         if (ftsensor.calibrate(c, rtStatus.deltaT))
    //         {
    //             ftsensor.calibrated.store(true);
    //         }
    //     }
    //     else
    //     {
    //         rtStatus.currentForceTorque = ftsensor.getFilteredForceTorque(c);
    //     }
    // }
 
    void
    TaskspaceAdmittanceController::run(Config& c, TSCtrlRtStatus& rtStatus)
    {
        /// run in rt thread
        rtStatus.currentPose = rtTCP->getPoseInRootFrame();
 
        /// ---------------------------- safety guard by FT and target pose ------------------------
        rtStatus.poseDiffMatImp =
            c.desiredPose.block<3, 3>(0, 0) * rtStatus.currentPose.block<3, 3>(0, 0).transpose();
        rtStatus.oriDiffAngleAxis.fromRotationMatrix(rtStatus.poseDiffMatImp);
 
        /// this should not be moved to non rt, just in case non rt thread dies, rt still functions as safely
        if (not ftsensor.isSafe(c.ftConfig))
        {
            ARMARX_INFO << "FTGuard indicates RT not safe";
            c.desiredPose = rtStatus.desiredPose;
            rtStatus.rtSafe = false;
            rtStatus.rtTargetSafe = true;
        }
        else if (ftsensor.calibrated.load())
        {
            if ((c.desiredPose.block<3, 1>(0, 3) - rtStatus.currentPose.block<3, 1>(0, 3)).norm() >
                    c.safeDistanceMMToGoal or
                std::fminf(rtStatus.oriDiffAngleAxis.angle(),
                           2 * M_PI - rtStatus.oriDiffAngleAxis.angle()) >
                    VirtualRobot::MathTools::deg2rad(c.safeRotAngleDegreeToGoal))
            {
                c.desiredPose = rtStatus.desiredPose;
                rtStatus.rtSafe = false;
                rtStatus.rtTargetSafe = false;
            }
            else
            {
                rtStatus.desiredPose = c.desiredPose;
                rtStatus.rtSafe = true;
                rtStatus.rtTargetSafe = true;
            }
        }
        else
        {
            ARMARX_RT_LOGF_WARN("not initialized or ft sensor not calibrated!");
            c.desiredTwist.setZero();
            c.kmAdmittance.setZero();
        }
 
        auto nDoF = static_cast<Eigen::Index>(rtStatus.nDoF);
 
        /// ------------------------------ compute jacobi ------------------------------------------
        rtStatus.jacobi =
            ik->getJacobianMatrix(rtTCP, VirtualRobot::IKSolver::CartesianSelection::All);
        rtStatus.jacobi.block(0, 0, 3, rtStatus.nDoF) =
            0.001 * rtStatus.jacobi.block(0, 0, 3, rtStatus.nDoF);
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.jacobi.cols());
        ARMARX_CHECK_EQUAL(6, rtStatus.jacobi.rows());
 
        rtStatus.jtpinv =
            ik->computePseudoInverseJacobianMatrix(rtStatus.jacobi.transpose(), rtStatus.lambda);
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.jtpinv.cols());
        ARMARX_CHECK_EQUAL(6, rtStatus.jtpinv.rows());
 
        /// ------------------------------ update velocity/twist -----------------------------------
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.jointPosition.rows());
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.jointVelocity.rows());
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.qvelFiltered.rows());
 
        rtStatus.qvelFiltered =
            (1 - c.qvelFilter) * rtStatus.qvelFiltered + c.qvelFilter * rtStatus.jointVelocity;
        rtStatus.currentTwist = rtStatus.jacobi * rtStatus.qvelFiltered;
 
        /// ---------------------------- admittance control ---------------------------------------------
        /// calculate pose error between the virtual pose and the target pose
        rtStatus.poseDiffMatAdm = rtStatus.virtualPose.block<3, 3>(0, 0) *
                                  rtStatus.desiredPose.block<3, 3>(0, 0).transpose();
        rtStatus.poseErrorAdm.head<3>() =
            rtStatus.virtualPose.block<3, 1>(0, 3) - rtStatus.desiredPose.block<3, 1>(0, 3);
        rtStatus.poseErrorAdm.tail<3>() =
            VirtualRobot::MathTools::eigen3f2rpy(rtStatus.poseDiffMatAdm);
 
        /// admittance control law and Euler Integration -> virtual pose
        rtStatus.acc = c.kmAdmittance.cwiseProduct(rtStatus.currentForceTorque) -
                       c.kpAdmittance.cwiseProduct(rtStatus.poseErrorAdm) -
                       c.kdAdmittance.cwiseProduct(rtStatus.virtualVel);
 
        rtStatus.vel =
            rtStatus.virtualVel + 0.5 * rtStatus.deltaT * (rtStatus.acc + rtStatus.virtualAcc);
        rtStatus.pos = 0.5 * rtStatus.deltaT * (rtStatus.vel + rtStatus.virtualVel);
        rtStatus.virtualAcc = rtStatus.acc;
        rtStatus.virtualVel = rtStatus.vel;
 
        rtStatus.virtualPose.block<3, 1>(0, 3) += rtStatus.pos.head<3>();
        rtStatus.virtualPose.block<3, 3>(0, 0) =
            VirtualRobot::MathTools::rpy2eigen3f(
                rtStatus.pos(3), rtStatus.pos(4), rtStatus.pos(5)) *
            rtStatus.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.
 
        rtStatus.poseDiffMatImp = rtStatus.desiredPose.block<3, 3>(0, 0) *
                                  rtStatus.currentPose.block<3, 3>(0, 0).transpose();
        rtStatus.poseErrorImp.head<3>() = 0.001 * (rtStatus.desiredPose.block<3, 1>(0, 3) -
                                                 rtStatus.currentPose.block<3, 1>(0, 3));
        rtStatus.poseErrorImp.tail<3>() =
            VirtualRobot::MathTools::eigen3f2rpy(rtStatus.poseDiffMatImp);
 
        rtStatus.forceImpedance = c.kpImpedance.cwiseProduct(rtStatus.poseErrorImp) -
                                  c.kdImpedance.cwiseProduct(rtStatus.currentTwist);
 
        /// ----------------------------- Nullspace PD Control -------------------------------------
        ARMARX_CHECK_EQUAL(nDoF, c.kpNullspaceTorque.rows());
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.nullspaceTorque.rows());
        ARMARX_CHECK_EQUAL(nDoF, c.desiredNullspaceJointAngles.value().size());
        rtStatus.nullspaceTorque =
                c.kpNullspaceTorque.cwiseProduct(c.desiredNullspaceJointAngles.value() - rtStatus.jointPosition) -
                c.kdNullspaceTorque.cwiseProduct(rtStatus.qvelFiltered);
 
        /// ----------------------------- Map TS target force to JS --------------------------------
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.desiredJointTorque.rows());
        rtStatus.desiredJointTorque =
            rtStatus.jacobi.transpose() * rtStatus.forceImpedance +
            (rtStatus.I - rtStatus.jacobi.transpose() * rtStatus.jtpinv) * rtStatus.nullspaceTorque;
 
        /// ----------------------------- write torque target  -------------------------------------
        for (int i = 0; i < rtStatus.desiredJointTorque.rows(); ++i)
        {
            rtStatus.desiredJointTorque(i) =
                std::clamp(rtStatus.desiredJointTorque(i), -c.torqueLimit, c.torqueLimit);
        }
    }
 
    // void
    // TaskspaceAdmittanceController::RtStatus::reset(const unsigned int nDoF)
    // {
    //     /// This function can be called before the RT thread.
    //     /// base
    //     jointPosition.setZero(nDoF);
    //     jointVelocity.setZero(nDoF);
    //     jointTorque.setZero(nDoF);
    //     deltaT = 0.0;
    //     accumulateTime = 0.0;
    //     nDoF = nDoF;
    //
    //     /// targets
    //     desiredJointTorque.setZero(nDoF);
    //     nullspaceTorque.setZero(nDoF);
    //
    //     /// force torque
    //     currentForceTorque.setZero();
    //
    //     /// task space variables
    //     forceImpedance.setZero();
    //
    //     /// current status
    //     qvelFiltered.setZero(nDoF);
    //     currentPose.setZero();
    //     currentTwist.setZero();
    //     desiredPose.setZero();
    //
    //     /// virtual status
    //     virtualPose.setZero();
    //     virtualVel.setZero();
    //     virtualAcc.setZero();
    //
    //     /// intermediate results
    //     poseDiffMatImp.setZero();
    //     poseDiffMatAdm.setZero();
    //     poseErrorImp.setZero();
    //     poseErrorAdm.setZero();
    //     oriDiffAngleAxis = Eigen::AngleAxisf::Identity();
    //     ARMARX_INFO << "------ " << VAROUT(oriDiffAngleAxis.angle());
    //
    //     /// others
    //     jacobi.setZero(6, nDoF);
    //     jtpinv.setZero(6, nDoF);
    //     rtSafe = false;
    //     rtTargetSafe = true;
    // }
    //
    // void
    // TaskspaceAdmittanceController::RtStatus::rtPreActivate(const Eigen::Matrix4f& currentTCPPose)
    // {
    //     /// !!!! TO BE UPDATED IN PREACTIVATE !!!!
    //     /// only then the up-to-date robot joint information is available
    //     currentPose = currentTCPPose;
    //     desiredPose = currentTCPPose;
    //     accumulateTime = 0.0;
    // }
 
} // namespace armarx::control::common::control_law