ZeroTorqueOrVelocityController.cpp

Go to the documentation of this file.

#include "ZeroTorqueOrVelocityController.h"
 
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/IK/IKSolver.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
 
#include "../utils.h"
 
namespace armarx::control::common::control_law
{
 
    void
    ZeroTorqueOrVelocityController::initialize(const VirtualRobot::RobotNodeSetPtr& rns,
                                               const VirtualRobot::RobotNodeSetPtr& rtRns,
                                               const std::vector<size_t>& torqueControlledIndex,
                                               const std::vector<size_t>& velocityControlledIndex)
    {
        ARMARX_CHECK_NOT_NULL(rns);
        // auto numOfJoints = rns->getSize();
 
        jointIDTorqueMode = torqueControlledIndex;
        jointIDVelocityMode = velocityControlledIndex;
        //        ARMARX_CHECK_EQUAL(jointIDTorqueMode.size() + jointIDVelocityMode.size(), numOfJoints);
        // nDoFTorque = jointIDTorqueMode.size();
        // nDoFVelocity = jointIDVelocityMode.size();
 
        tcp = rns->getTCP();
        rtTCP = rtRns->getTCP();
        ARMARX_CHECK_NOT_NULL(tcp);
        ik.reset(new VirtualRobot::DifferentialIK(
            rtRns, rtRns->getRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
 
 
        // I = Eigen::MatrixXf::Identity(numOfJoints, numOfJoints);
    }
 
    void
    ZeroTorqueOrVelocityController::preactivateInit(const VirtualRobot::RobotNodeSetPtr& rns)
    {
    }
 
    void
    ZeroTorqueOrVelocityController::firstRun()
    {
        ftsensor.reset();
    }
 
    //    bool
    //    ZeroTorqueOrVelocityController::isControlModeValid(
    //        const std::vector<std::string>& nameList,
    //        const std::map<std::string, std::string>& jointControlModeMap)
    //    {
    //        std::set<std::string> validModes = {"velocity", "torque"};
 
    //        for (const auto& pair : jointControlModeMap)
    //        {
    //            if (std::find(nameList.begin(), nameList.end(), pair.first) == nameList.end())
    //            {
    //                ARMARX_ERROR << "ZeroTorqueOrVelocityController: joint name '" << pair.first
    //                             << "' not found in robot node set.";
    //                return false;
    //            }
 
    //            if (validModes.find(pair.second) == validModes.end())
    //            {
    //                ARMARX_ERROR << "ZeroTorqueOrVelocityController: Invalid control mode for joint '"
    //                             << pair.first << "': " << pair.second << std::endl;
    //                return false;
    //            }
    //        }
 
    //        return true;
    //    }
 
    // void
    // ZeroTorqueOrVelocityController::updateFT(const ft::arondto::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
    ZeroTorqueOrVelocityController::run(Config& c, TSCtrlRtStatus& rtStatus)
    {
        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, nDoF) =
            0.001 * rtStatus.jacobi.block(0, 0, 3, nDoF);
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.jacobi.cols());
        ARMARX_CHECK_EQUAL(6, rtStatus.jacobi.rows());
 
        rtStatus.jpinv = ik->computePseudoInverseJacobianMatrix(rtStatus.jacobi, rtStatus.lambda);
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.jpinv.rows());
        ARMARX_CHECK_EQUAL(6, rtStatus.jpinv.cols());
 
        /// ------------------------------ 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;
 
        // current twist is only modelled w.r.t. to two wrist joint
        rtStatus.currentTwist.setZero();
        for (size_t i = 0; i < 6; i++)
        {
            for (auto j : jointIDVelocityMode)
            {
                rtStatus.currentTwist(i) += rtStatus.jacobi(i, j) * rtStatus.qvelFiltered(j);
            }
        }
 
        /// ------------------------------------- Velocity control ---------------------------------------------------
        /// TODO add doc
        rtStatus.cartesianVelTarget.setZero();
        if (rtStatus.currentForceTorque.head<3>().norm() > c.forceAmplitudeDeadZone)
        {
            rtStatus.cartesianVelTarget.head<3>() =
                c.kpCartesianVel.head<3>().cwiseProduct(rtStatus.currentForceTorque.head<3>()) -
                c.kdCartesianVel.head<3>().cwiseProduct(rtStatus.currentTwist.head<3>());
        }
        if (rtStatus.currentForceTorque.tail<3>().norm() > c.torqueAmplitudeDeadZone)
        {
            rtStatus.cartesianVelTarget.tail<3>() =
                c.kpCartesianVel.tail<3>().cwiseProduct(rtStatus.currentForceTorque.tail<3>()) -
                c.kdCartesianVel.tail<3>().cwiseProduct(rtStatus.currentTwist.tail<3>());
        }
 
 
        /// ----------------------------- Map TS target force/velocity to JS --------------------------------------------------
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.desiredJointTorque.rows());
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.desiredJointVelocity.rows());
        rtStatus.desiredJointTorque.setZero();
        rtStatus.desiredJointVelocity = rtStatus.jpinv * rtStatus.cartesianVelTarget;
 
        /// ----------------------------- write torque/velocity target  --------------------------------------------------
        for (int i = 0; i < rtStatus.desiredJointVelocity.rows(); ++i)
        {
            rtStatus.desiredJointVelocity(i) =
                std::clamp(rtStatus.desiredJointVelocity(i), -c.velocityLimit, c.velocityLimit);
        }
    }
} // namespace armarx::control::common::control_law