TaskspaceController.cpp

Go to the documentation of this file.

#include "TaskspaceController.h"
 
#include <algorithm>
#include <cmath>
#include <map>
#include <set>
#include <string>
 
#include <Eigen/Core>
 
#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/SensorValues/SensorValue1DoFActuator.h>
 
#include <armarx/control/common/device.h>
 
namespace armarx::control::common::control_law
{
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::firstRun()
    {
        ftsensor.reset();
    }
 
    template <typename ConfigType, typename ConfigDictType>
    bool
    TaskspaceController<ConfigType, ConfigDictType>::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 << "TaskspaceController: joint name '" << pair.first
                             << "' not found in robot node set.";
                return false;
            }
 
            if (validModes.find(pair.second) == validModes.end())
            {
                ARMARX_ERROR << "TaskspaceController: Invalid control mode for joint '"
                             << pair.first << "': " << pair.second << std::endl;
                return false;
            }
        }
 
        return true;
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::validate(ConfigType& c,
                                                              TSCtrlRtStatus& rtStatus)
    {
        /// run in rt thread
        rtStatus.currentPose = rtTCP->getPoseInRootFrame();
 
        /// safety guard by FT and target pose
        rtStatus.poseDiffMatImp.noalias() = c.desiredPose.template 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 safely
        rtStatus.forceTorqueSafe = ftsensor.isSafe(c.ftConfig);
        if (not rtStatus.forceTorqueSafe)
        {
            if (c.ftConfig.resetToCurrentPoseOnSafeGuardTrigger)
            {
                c.desiredPose = rtStatus.currentPose;
            }
            else
            {
                c.desiredPose = rtStatus.desiredPose;
            }
        }
 
        // if ((c.desiredPose.block<3, 1>(0, 3) - rtStatus.currentPose.block<3, 1>(0, 3)).norm() >
        if ((rtStatus.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))
        {
            rtStatus.desiredPoseUnsafe = c.desiredPose;
            c.desiredPose = rtStatus.desiredPose;
            rtStatus.rtTargetSafe = false;
        }
        else
        {
            rtStatus.rtTargetSafe = true;
        }
 
        rtStatus.rtSafe = rtStatus.forceTorqueSafe and rtStatus.rtTargetSafe;
        if (rtStatus.rtSafe)
        {
            rtStatus.desiredPose = c.desiredPose;
        }
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::updateJacobian(ConfigType& c,
                                                                    TSCtrlRtStatus& rtStatus)
    {
        auto nDoF = static_cast<Eigen::Index>(rtStatus.nDoF);
        /// ------------------------------ compute jacobi ------------------------------------------
        // rtStatus.jacobi =
        //     ik->getJacobianMatrix(rtTCP, VirtualRobot::IKSolver::CartesianSelection::All);
        ik->updateJacobianMatrix(
            rtStatus.jacobi, rtTCP, VirtualRobot::IKSolver::CartesianSelection::All);
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.jacobi.cols());
        ARMARX_CHECK_EQUAL(6, rtStatus.jacobi.rows());
 
 
        rtStatus.jacobi.block(0, 0, 3, rtStatus.nDoF) =
            0.001 * rtStatus.jacobi.block(0, 0, 3, rtStatus.nDoF);
 
        // rtStatus.jpinv = ik->computePseudoInverseJacobianMatrix(rtStatus.jacobi, lambda);
        ik->updatePseudoInverseJacobianMatrix(
            rtStatus.jpinv, rtStatus.jacobi, 0.f, JacPseudoInvRegularization);
        ARMARX_CHECK_EQUAL(6, rtStatus.jpinv.cols());
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.jpinv.rows());
 
        //        rtStatus.jtpinv =
        //            ik->computePseudoInverseJacobianMatrix(rtStatus.jacobi.transpose(), rtStatus.lambda);
        rtStatus.jtpinv = ik->computePseudoInverseJacobianMatrix(rtStatus.jacobi.transpose(),
                                                                 JacPseudoInvRegularization);
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.jtpinv.cols());
        ARMARX_CHECK_EQUAL(6, rtStatus.jtpinv.rows());
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::updateTwist(ConfigType& c,
                                                                 TSCtrlRtStatus& rtStatus)
    {
        auto nDoF = static_cast<Eigen::Index>(rtStatus.nDoF);
        /// ------------------------------ 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; /// in m/s
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::updateTSError(ConfigType& c,
                                                                   TSCtrlRtStatus& rtStatus)
    {
        /// ------------------------------------- TS Error --------------------------------
        /// calculate pose error between target pose and current pose
        /// !!! This is very important: you have to keep position 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();
        /// position error in meter
        rtStatus.poseErrorImp.head<3>() = 0.001 * (rtStatus.desiredPose.block<3, 1>(0, 3) -
                                                   rtStatus.currentPose.block<3, 1>(0, 3));
        //        rtStatus.poseErrorImp.head<3>() = (rtStatus.desiredPose.block<3, 1>(0, 3) -
        //                                           rtStatus.currentPose.block<3, 1>(0, 3)); /// in mm
        rtStatus.poseErrorImp.tail<3>() =
            VirtualRobot::MathTools::eigen3f2rpy(rtStatus.poseDiffMatImp);
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::updateTSTwistCommand(ConfigType& c,
                                                                          TSCtrlRtStatus& rtStatus)
    {
        /// --------------------------- Velocity control -------------------------------------------
        /// Unit in m/s and rad/s
        rtStatus.cartesianVelTarget = c.kpCartesianVel.cwiseProduct(rtStatus.poseErrorImp) -
                                      c.kdCartesianVel.cwiseProduct(rtStatus.currentTwist);
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::updateTSForceCommand(ConfigType& c,
                                                                          TSCtrlRtStatus& rtStatus)
    {
        //        rtStatus.poseErrorImp.head<3>() *= 0.001;
        /// Unit in N, and Nm
        rtStatus.forceImpedance = c.kpImpedance.cwiseProduct(rtStatus.poseErrorImp) -
                                  c.kdImpedance.cwiseProduct(rtStatus.currentTwist);
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::updateNullspaceForceCommand(
        ConfigType& c,
        TSCtrlRtStatus& rtStatus)
    {
        auto nDoF = static_cast<Eigen::Index>(rtStatus.nDoF);
        ARMARX_CHECK_EQUAL(nDoF, c.kpNullspaceTorque.rows());
        ARMARX_CHECK_EQUAL(nDoF, c.kdNullspaceTorque.rows());
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.nullspaceTorque.rows());
        ARMARX_CHECK_EQUAL(nDoF, c.desiredNullspaceJointAngles.value().size());
        /// Nullspace PD Control
        rtStatus.nullspaceTorque =
            c.kpNullspaceTorque.cwiseProduct(c.desiredNullspaceJointAngles.value() -
                                             rtStatus.jointPosition) -
            c.kdNullspaceTorque.cwiseProduct(rtStatus.qvelFiltered);
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::updateNullspaceTwistCommand(
        ConfigType& c,
        TSCtrlRtStatus& rtStatus)
    {
        auto nDoF = static_cast<Eigen::Index>(rtStatus.nDoF);
        ARMARX_CHECK_EQUAL(nDoF, c.kpNullspaceVel.rows());
        ARMARX_CHECK_EQUAL(nDoF, c.kdNullspaceVel.rows());
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.nullspaceVelocity.rows());
        ARMARX_CHECK_EQUAL(nDoF, c.desiredNullspaceJointAngles.value().size());
        /// Nullspace PD Control
        rtStatus.nullspaceVelocity =
            c.kpNullspaceVel.cwiseProduct(c.desiredNullspaceJointAngles.value() -
                                          rtStatus.jointPosition) -
            c.kdNullspaceVel.cwiseProduct(rtStatus.qvelFiltered);
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::applyInverseDynamics(ConfigType& c,
                                                                          TSCtrlRtStatus& rtStatus)
    {
        auto nDoF = static_cast<Eigen::Index>(rtStatus.nDoF);
        /// 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);
        }
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::applyInverseKinematics(
        ConfigType& c,
        TSCtrlRtStatus& rtStatus)
    {
        auto nDoF = static_cast<Eigen::Index>(rtStatus.nDoF);
        /// Map TS target velocity to JS
        ARMARX_CHECK_EQUAL(nDoF, rtStatus.desiredJointVelocity.rows());
        rtStatus.desiredJointVelocity =
            rtStatus.jpinv * rtStatus.cartesianVelTarget +
            (rtStatus.I - rtStatus.jpinv * rtStatus.jacobi) * rtStatus.nullspaceVelocity;
 
        for (int i = 0; i < rtStatus.desiredJointVelocity.rows(); ++i)
        {
            rtStatus.desiredJointVelocity(i) =
                std::clamp(rtStatus.desiredJointVelocity(i), -c.velocityLimit, c.velocityLimit);
        }
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::setForceTorqueGuard(const bool forceGuard,
                                                                         const bool torqueGuard)
    {
        ftsensor.enableSafeGuard(forceGuard, torqueGuard);
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::updateTSAdmittanceCommand(
        ConfigType& c,
        TSCtrlRtStatus& rtStatus)
    {
        /// ---------------------------- 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 position 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);
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::computeInertiaInverse(TSCtrlRtStatus& rtStatus)
    {
 
        /// Determine the size of the submatrix (M x M)
        auto& idx = rtStatus.jointIDVelocityMode;
        size_t m = idx.size();
 
        /// Extract elements: Loop over rows and columns
        for (size_t i = 0; i < m; ++i)
        {
            for (size_t j = 0; j < m; ++j)
            {
                // Map the submatrix (i, j) to the original matrix (indices[i], indices[j])
                rtStatus.inertiaVelCtrlJoints(i, j) = rtStatus.inertia(idx[i], idx[j]);
            }
        }
        //        ARMARX_INFO << VAROUT(rtStatus.inertiaVelCtrlJoints);
        double epsilon = 1e-5;
        if (rtStatus.inertiaVelCtrlJoints.isZero(epsilon))
        {
            rtStatus.inertiaInvVelCtrlJoints.setZero();
            return;
        }
        /// Invert and cast to float
        rtStatus.inertiaInvVelCtrlJoints =
            rtStatus.inertiaVelCtrlJoints.cast<float>().llt().solve(rtStatus.IVelCtrlJoint);
        if (rtStatus.inertiaVelCtrlJoints.hasNaN())
        {
            rtStatus.inertiaInvVelCtrlJoints.setZero();
        }
    }
 
    template <typename ConfigType, typename ConfigDictType>
    void
    TaskspaceController<ConfigType, ConfigDictType>::applyAdmittanceInterface(
        ConfigType& c,
        TSCtrlRtStatus& rtStatus)
    {
 
        float k_d = 0.5;
 
        size_t m = rtStatus.nDoFVelocity;
        for (size_t i = 0; i < m; ++i)
        {
            size_t idx = rtStatus.jointIDVelocityMode[i];
            rtStatus.qTemp(i) = rtStatus.desiredJointTorque(idx) -
                                k_d * (rtStatus.qdVel(i) - rtStatus.qvelFiltered(idx));
            /* - k_p * (rtStatus.qdPos(i) - rtStatus.jointPosition(idx))*/
        }
        rtStatus.qdAcc = rtStatus.inertiaInvVelCtrlJoints * rtStatus.qTemp;
        rtStatus.qdVel = 0.5 * (rtStatus.deltaT * rtStatus.qdAcc + rtStatus.qdVel);
        rtStatus.desiredJointVelocity.setZero();
        for (size_t i = 0; i < m; ++i)
        {
            rtStatus.desiredJointVelocity(rtStatus.jointIDVelocityMode[i]) =
                std::clamp(rtStatus.qdVel(i), -c.velocityLimit, c.velocityLimit);
        }
    }
 
    template <typename ConfigType, typename ConfigDictType>
    bool
    TaskspaceController<ConfigType, ConfigDictType>::isForceTorqueSafe()
    {
        return ftsensor.ftSafe.load();
    }
 
    void
    TSMixImpVelController::run(Config& c, TSCtrlRtStatus& rtStatus)
    {
        validate(c, rtStatus);
        updateJacobian(c, rtStatus);
        updateTwist(c, rtStatus);
        updateTSError(c, rtStatus);
 
        if (not c.enableAdmInterface)
        {
            updateTSTwistCommand(c, rtStatus);
            updateNullspaceTwistCommand(c, rtStatus);
        }
 
        updateTSForceCommand(c, rtStatus);
        updateNullspaceForceCommand(c, rtStatus);
 
        applyInverseDynamics(c, rtStatus);
        if (c.enableAdmInterface)
        {
            applyAdmittanceInterface(c, rtStatus);
        }
        else
        {
            applyInverseKinematics(c, rtStatus);
        }
    }
 
    //    void
    //    TSMixImpVelController::run(Config& c, TSCtrlRtStatus& rtStatus)
    //    {
    //        validate(c, rtStatus);
    //        updateJacobian(c, rtStatus);
    //        updateTwist(c, rtStatus);
    //        updateTSError(c, rtStatus);
 
    //        //        updateTSTwistCommand(c, rtStatus);
    //        //        updateNullspaceTwistCommand(c, rtStatus);
 
    //        updateTSForceCommand(c, rtStatus);
    //        updateNullspaceForceCommand(c, rtStatus);
 
    //        applyInverseDynamics(c, rtStatus);
    //        computeInertiaInverse(rtStatus);
    //        applyAdmittanceInterface(c, rtStatus);
    //    }
 
    void
    TSImpController::run(Config& c, TSCtrlRtStatus& rtStatus)
    {
        validate(c, rtStatus);
        updateJacobian(c, rtStatus);
        updateTwist(c, rtStatus);
        updateTSError(c, rtStatus);
 
        updateTSForceCommand(c, rtStatus);
        updateNullspaceForceCommand(c, rtStatus);
 
        applyInverseDynamics(c, rtStatus);
    }
 
    void
    TSVelController::run(Config& c, TSCtrlRtStatus& rtStatus)
    {
        validate(c, rtStatus);
        updateJacobian(c, rtStatus);
        updateTwist(c, rtStatus);
        updateTSError(c, rtStatus);
 
        updateTSTwistCommand(c, rtStatus);
        updateNullspaceTwistCommand(c, rtStatus);
 
        computeInertiaInverse(rtStatus);
        applyInverseKinematics(c, rtStatus);
    }
 
    void
    TSAdmController::run(Config& c, TSCtrlRtStatus& rtStatus)
    {
        validate(c, rtStatus);
        updateJacobian(c, rtStatus);
        updateTwist(c, rtStatus);
        updateTSAdmittanceCommand(c, rtStatus);
        updateNullspaceForceCommand(c, rtStatus);
 
        applyInverseDynamics(c, rtStatus);
    }
 
} // namespace armarx::control::common::control_law