TaskspaceMixedImpedanceVelocityController.cpp

Go to the documentation of this file.

#include "TaskspaceMixedImpedanceVelocityController.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
    TaskspaceMixedImpedanceVelocityController::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);
        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
    TaskspaceMixedImpedanceVelocityController::preactivateInit(
        const VirtualRobot::RobotNodeSetPtr& rns)
    {
    }
 
    void
    TaskspaceMixedImpedanceVelocityController::firstRun()
    {
        ftsensor.reset();
    }
 
    bool
    TaskspaceMixedImpedanceVelocityController::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 << "TaskspaceMixedImpedanceVelocityController: joint name '"
                             << pair.first << "' not found in robot node set.";
                return false;
            }
 
            if (validModes.find(pair.second) == validModes.end())
            {
                ARMARX_ERROR
                    << "TaskspaceMixedImpedanceVelocityController: Invalid control mode for joint '"
                    << pair.first << "': " << pair.second << std::endl;
                return false;
            }
        }
 
        return true;
    }
 
    // void
    // TaskspaceMixedImpedanceVelocityController::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
    TaskspaceMixedImpedanceVelocityController::run(Config& c, RtStatus& 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
        rtStatus.forceTorqueSafe = ftsensor.isSafe(c.ftConfig);
        if (not rtStatus.forceTorqueSafe)
        {
            ARMARX_INFO << "FTGuard indicates RT not safe";
            if (c.ftConfig.resetToCurrentPoseOnSafeGuardTrigger)
            {
                ARMARX_INFO << "reset to current pose " << rtStatus.currentPose;
                c.desiredPose = rtStatus.currentPose;
            }
            else
            {
                //                ARMARX_INFO << "reset to valid target pose " << rtStatus.desiredPose;
                c.desiredPose = rtStatus.desiredPose;
            }
            //            rtStatus.forceTorqueSafe = false;
            //            rtStatus.rtSafe = false;
            //            rtStatus.rtTargetSafe = true;
        }
 
        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))
        {
            rtStatus.desiredPoseUnsafe = c.desiredPose;
            c.desiredPose = rtStatus.desiredPose; // TODO: remove this?
            //            rtStatus.rtSafe = false;
            rtStatus.rtTargetSafe = false;
            //            rtStatus.forceTorqueSafe = true;
        }
        else
        {
            rtStatus.rtTargetSafe = true;
        }
 
        rtStatus.rtSafe = rtStatus.forceTorqueSafe and rtStatus.rtTargetSafe;
        if (rtStatus.rtSafe)
        {
            rtStatus.desiredPose = c.desiredPose;
            //            rtStatus.rtSafe = true;
            //            rtStatus.rtTargetSafe = true;
            //            rtStatus.forceTorqueSafe = true;
        }
 
        /// ------------------------------ compute jacobi ------------------------------------------
        rtStatus.jacobi =
            ik->getJacobianMatrix(rtTCP, VirtualRobot::IKSolver::CartesianSelection::All);
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jacobi.cols());
        ARMARX_CHECK_EQUAL(6, rtStatus.jacobi.rows());
 
        // rtStatus.jpinv = ik->computePseudoInverseJacobianMatrix(rtStatus.jacobi, lambda);
        rtStatus.jpinv = ik->computePseudoInverseJacobianMatrix(
            rtStatus.jacobi,
            ik->getJacobiRegularization(VirtualRobot::IKSolver::CartesianSelection::All));
        ARMARX_CHECK_EQUAL(6, rtStatus.jpinv.cols());
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jpinv.rows());
 
        rtStatus.jacobi.block(0, 0, 3, rtStatus.numJoints) =
            0.001 * rtStatus.jacobi.block(0, 0, 3, rtStatus.numJoints);
 
        rtStatus.jtpinv =
            ik->computePseudoInverseJacobianMatrix(rtStatus.jacobi.transpose(), lambda);
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jtpinv.cols());
        ARMARX_CHECK_EQUAL(6, rtStatus.jtpinv.rows());
 
        /// ------------------------------ update velocity/twist -----------------------------------
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointPos.rows());
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.jointVel.rows());
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.qvelFiltered.rows());
 
        rtStatus.qvelFiltered =
            (1 - c.qvelFilter) * rtStatus.qvelFiltered + c.qvelFilter * rtStatus.jointVel;
        rtStatus.currentTwist = rtStatus.jacobi * rtStatus.qvelFiltered;
 
        /// ------------------------------------- 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.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);
 
        /// ------------------------------------- Velocity 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 desired Cartesian velocity.
        rtStatus.cartesianVelTarget = c.kpCartesianVel.cwiseProduct(rtStatus.poseErrorImp) -
                                      c.kdCartesianVel.cwiseProduct(rtStatus.currentTwist);
 
        rtStatus.poseErrorImp.head<3>() *= 0.001;
        rtStatus.forceImpedance = c.kpImpedance.cwiseProduct(rtStatus.poseErrorImp) -
                                  c.kdImpedance.cwiseProduct(rtStatus.currentTwist);
 
 
        /// ----------------------------- Nullspace PD Control --------------------------------------------------
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, c.kpNullspaceTorque.rows());
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, c.kdNullspaceTorque.rows());
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.nullspaceTorque.rows());
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, c.kpNullspaceVel.rows());
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, c.kdNullspaceVel.rows());
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.nullspaceVelocity.rows());
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, c.desiredNullspaceJointAngles.value().size());
 
        rtStatus.nullspaceTorque = c.kpNullspaceTorque.cwiseProduct(
                                       c.desiredNullspaceJointAngles.value() - rtStatus.jointPos) -
                                   c.kdNullspaceTorque.cwiseProduct(rtStatus.jointVel);
        rtStatus.nullspaceVelocity =
            c.kpNullspaceVel.cwiseProduct(c.desiredNullspaceJointAngles.value() -
                                          rtStatus.jointPos) -
            c.kdNullspaceVel.cwiseProduct(rtStatus.qvelFiltered);
 
        /// ----------------------------- Map TS target force/velocity to JS --------------------------------------------------
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.desiredJointTorques.rows());
        ARMARX_CHECK_EQUAL(rtStatus.numJoints, rtStatus.desiredJointVelocity.rows());
        rtStatus.desiredJointTorques =
            rtStatus.jacobi.transpose() * rtStatus.forceImpedance +
            (I - rtStatus.jacobi.transpose() * rtStatus.jtpinv) * rtStatus.nullspaceTorque;
        rtStatus.desiredJointVelocity =
            rtStatus.jpinv * rtStatus.cartesianVelTarget +
            (I - rtStatus.jpinv * rtStatus.jacobi) * rtStatus.nullspaceVelocity;
 
        /// ----------------------------- write torque target  -------------------------------------
        for (int i = 0; i < rtStatus.desiredJointTorques.rows(); ++i)
        {
            rtStatus.desiredJointTorques(i) =
                std::clamp(rtStatus.desiredJointTorques(i), -c.torqueLimit, c.torqueLimit);
        }
        for (int i = 0; i < rtStatus.desiredJointVelocity.rows(); ++i)
        {
            rtStatus.desiredJointVelocity(i) =
                std::clamp(rtStatus.desiredJointVelocity(i), -c.velocityLimit, c.velocityLimit);
        }
    }
 
    void
    TaskspaceMixedImpedanceVelocityController::RtStatus::reset(const unsigned int nDoF,
                                                               const size_t numDoFTorque,
                                                               const size_t numDoFVelocity)
    {
        /// This function can be called before the RT thread.
        /// base
        jointPosition.resize(nDoF, 0.);
        jointVelocity.resize(nDoF, 0.);
        jointTorque.resize(nDoF, 0.);
        jointPos.setZero(nDoF);
        jointVel.setZero(nDoF);
        jointTor.setZero(nDoF);
        deltaT = 0.0;
        accumulateTime = 0.0;
        numJoints = nDoF;
        nDoFTorque = numDoFTorque;
        nDoFVelocity = numDoFVelocity;
        ARMARX_CHECK_EQUAL(numJoints, nDoFTorque + nDoFVelocity);
 
        /// targets
        desiredJointTorques.setZero(nDoF);
        desiredJointVelocity.setZero(nDoF);
        cartesianVelTarget.setZero();
        nullspaceTorque.setZero(nDoF);
        nullspaceVelocity.setZero(nDoF);
 
        /// force torque
        currentForceTorque.setZero();
        safeFTGuardOffset.setZero();
 
        /// task space variables
        forceImpedance.setZero();
 
        /// current status
        qvelFiltered.setZero(nDoF);
        currentPose.setZero();
        currentTwist.setZero();
        desiredPose.setZero();
        desiredPoseUnsafe.setZero();
 
        /// intermediate results
        poseDiffMatImp.setZero();
        poseErrorImp.setZero();
        oriDiffAngleAxis = Eigen::AngleAxisf::Identity();
 
        /// others
        jacobi.setZero(6, nDoF);
        jtpinv.setZero(6, nDoF);
        jpinv.setZero(nDoF, 6);
        rtSafe = false;
        rtTargetSafe = true;
    }
 
    void
    TaskspaceMixedImpedanceVelocityController::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