KeypointsAdmittanceController.cpp

Go to the documentation of this file.

#include "KeypointsAdmittanceController.h"
 
#include <fstream>
 
#include <boost/algorithm/clamp.hpp>
 
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/IK/JacobiProvider.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.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 "../utils.h"
 
namespace armarx::control::common::control_law
{
 
    void
    KeypointsAdmittanceController::initialize(const VirtualRobot::RobotNodeSetPtr& rns,
                                              const std::string& configFileName)
    {
        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();
        s.qpos.resize(numOfJoints);
        s.qvel.resize(numOfJoints);
        s.desiredJointTorques.resize(numOfJoints);
 
        s.currentForceTorque.setZero();
        s.currentTwist.setZero();
        s.forceImpedance.setZero();
        s.virtualVel.setZero();
        s.virtualAcc.setZero();
        s.pointTrackingForce.setZero();
 
        /// parse configuration files
        nlohmann::json userConfig;
        nlohmann::json defaultConfig;
        std::string filename;
        {
            ArmarXDataPath::getAbsolutePath("armarx_control/controller_config/default/"
                                            "njoint_controller/impedance_controller_config.json",
                                            filename);
            ARMARX_INFO << "reading default config from: " << VAROUT(filename);
            std::ifstream ifs{filename};
            ifs >> defaultConfig;
            if (defaultConfig.empty())
            {
                ARMARX_ERROR << "default config is empty!";
            }
        }
        if (configFileName.empty())
        {
            ARMARX_IMPORTANT
                << "No control parameter specified by user, use default parameter in \n"
                << filename;
            userConfig = defaultConfig;
        }
        else
        {
            std::ifstream ifs{configFileName};
            ifs >> userConfig;
        }
        if (userConfig.empty())
        {
            ARMARX_ERROR << "userConfig is empty!";
        }
 
        if (userConfig.find("control") != userConfig.end())
        {
            auto c = userConfig["control"];
            auto dc = defaultConfig["control"];
            s.kpImpedance = getEigenVec(c, dc, "impedance_stiffness");
            s.kdImpedance = getEigenVec(c, dc, "impedance_damping");
            s.kpAdmittance = getEigenVec(c, dc, "admittance_stiffness");
            s.kdAdmittance = getEigenVec(c, dc, "admittance_damping");
            s.kmAdmittance = getEigenVec(c, dc, "admittance_inertia");
            s.kpNullspace = getEigenVec(c, dc, "nullspace_stiffness", numOfJoints, 10.0f);
            s.kdNullspace = getEigenVec(c, dc, "nullspace_damping", numOfJoints, 2.0f);
            s.desiredNullspaceJointAngles = getEigenVec(c, dc, "desired_nullspace_joint_angles");
            if (s.desiredNullspaceJointAngles.size() != numOfJoints)
            {
                ARMARX_IMPORTANT << "default value is not consistent with requested kinematic "
                                    "chain, reinit in preactivation";
                enablePreactivateInit.store(true);
            }
            s.torqueLimit = getValue<float>(c, dc, "torque_limit");
            s.qvelFilter = getValue<float>(c, dc, "qvel_filter");
            /// keypoints related
            s.numPoints = getValue<int>(c, dc, "num_points");
            s.keypointKp = getEigenVec(c, dc, "keypoint_stiffness");
            s.keypointKd = getEigenVec(c, dc, "keypoint_damping");
            s.currentKeypointPosition = getEigenVec(c, dc, "current_keypoint_position");
            s.desiredKeypointPosition = s.currentKeypointPosition;
            s.filteredKeypointPosition = s.currentKeypointPosition;
            s.previousKeypointPosition = s.currentKeypointPosition;
            s.keypointPositionFilter = getValue<float>(c, dc, "keypoint_position_filter");
            s.keypointVelocityFilter = getValue<float>(c, dc, "keypoint_velocity_filter");
 
            s.isRigid = getValue<bool>(c, dc, "is_rigid");
            s.fixedTranslation = getEigenVec(c, dc, "fixed_translation");
            s.desiredKeypointVelocity.setZero(3 * s.numPoints);
            s.currentKeypointVelocity.setZero(3 * s.numPoints);
        }
        else
        {
            ARMARX_WARNING << "controller parameters not found, they should be placed in 'control' "
                              "tag in your json file";
        }
 
        I = Eigen::MatrixXf::Identity(numOfJoints, numOfJoints);
    }
 
    KeypointsAdmittanceController::Config
    KeypointsAdmittanceController::reconfigure(const std::string& configFileName)
    {
        nlohmann::json userConfig;
        std::ifstream ifs{configFileName};
        ifs >> userConfig;
        auto c = userConfig["control"];
        Config config = Config();
        config.kpImpedance = getEigenVecWithDefault(c, s.kpImpedance, "impedance_stiffness");
        config.kdImpedance = getEigenVecWithDefault(c, s.kdImpedance, "impedance_damping");
        config.kpAdmittance = getEigenVecWithDefault(c, s.kpAdmittance, "admittance_stiffness");
        config.kdAdmittance = getEigenVecWithDefault(c, s.kdAdmittance, "admittance_damping");
        config.kmAdmittance = getEigenVecWithDefault(c, s.kmAdmittance, "admittance_inertia");
        config.kpNullspace = getEigenVecWithDefault(c, s.kpNullspace, "nullspace_stiffness");
        config.kdNullspace = getEigenVecWithDefault(c, s.kdNullspace, "nullspace_damping");
        config.currentForceTorque.setZero();
        config.desiredNullspaceJointAngles = getEigenVecWithDefault(
            c, s.desiredNullspaceJointAngles, "desired_nullspace_joint_angles");
        config.torqueLimit = getValueWithDefault<float>(c, s.torqueLimit, "torque_limit");
        config.qvelFilter = getValueWithDefault<float>(c, s.qvelFilter, "qvel_filter");
        /// keypoint related
        config.numPoints = getValueWithDefault<int>(c, s.numPoints, "num_points");
        config.keypointKp = getEigenVecWithDefault(c, s.keypointKp, "keypoint_stiffness");
        config.keypointKd = getEigenVecWithDefault(c, s.keypointKd, "keypoint_damping");
        config.currentKeypointPosition =
            getEigenVecWithDefault(c, s.currentKeypointPosition, "current_keypoint_position");
        config.desiredKeypointPosition =
            getEigenVecWithDefault(c, config.currentKeypointPosition, "desired_keypoint_position");
        config.desiredKeypointVelocity.setZero(config.numPoints * 3);
        config.desiredKeypointVelocity =
            getEigenVecWithDefault(c, config.desiredKeypointVelocity, "desired_keypoint_velocity");
        config.keypointPositionFilter =
            getValueWithDefault<float>(c, s.keypointPositionFilter, "keypoint_position_filter");
        config.keypointVelocityFilter =
            getValueWithDefault<float>(c, s.keypointVelocityFilter, "keypoint_velocity_filter");
        config.isRigid = getValueWithDefault<bool>(c, s.isRigid, "is_rigid");
        config.fixedTranslation =
            getEigenVecWithDefault(c, s.fixedTranslation, "fixed_translation");
        return config;
    }
 
    void
    KeypointsAdmittanceController::preactivateInit(const VirtualRobot::RobotNodeSetPtr& rns)
    {
        Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame();
        s.qpos = rns->getJointValuesEigen();
        s.qvel.setZero(numOfJoints);
        s.nullspaceTorque.setZero(numOfJoints);
        s.desiredJointTorques.setZero(numOfJoints);
 
        if (s.isRigid)
        {
            for (int i = 0; i < s.numPoints; i++)
            {
                s.currentKeypointPosition.segment(3 * i, 3) = currentPose.block(0, 3, 3, 1);
            }
            s.currentKeypointPosition = s.currentKeypointPosition + s.fixedTranslation;
        }
 
        s.previousDesiredPose = currentPose;
        s.desiredPose = currentPose;
        s.desiredVel.setZero();
        s.desiredAcc.setZero();
 
        s.currentPose = currentPose;
        s.currentTwist.setZero();
        s.forceImpedance.setZero();
 
        s.virtualPose = currentPose;
        s.virtualVel.setZero();
        s.virtualAcc.setZero();
 
        s.pointTrackingForce.setZero();
        s.filteredKeypointPosition = s.currentKeypointPosition;
        s.previousKeypointPosition = s.currentKeypointPosition;
        s.currentKeypointVelocity.setZero(s.numPoints * 3);
        s.desiredKeypointVelocity.setZero(s.numPoints * 3);
 
        if (enablePreactivateInit.load())
        {
            s.desiredNullspaceJointAngles = rns->getJointValuesEigen();
        }
    }
 
    void
    KeypointsAdmittanceController::firstRun()
    {
        s.previousDesiredPose = s.currentPose;
        s.desiredPose = s.currentPose;
        s.desiredVel.setZero();
        s.desiredAcc.setZero();
 
        s.virtualPose = s.currentPose;
        s.virtualVel.setZero();
        s.virtualAcc.setZero();
    }
 
    bool
    KeypointsAdmittanceController::validateTargetPose(Eigen::Matrix4f& targetPose)
    {
        bool valid = false;
        if ((s.previousDesiredPose.block<3, 1>(0, 3) - targetPose.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(targetPose),
                                VAROUT(s.previousDesiredPose));
            //        ARMARX_WARNING << "new target \n\n" << targetPose << "\n\nis too far away from\n\n" << VAROUT(s.previousDesiredPose);
            targetPose = s.previousDesiredPose;
            s.currentForceTorque.setZero();
        }
        else
        {
            s.previousDesiredPose = targetPose;
            valid = true;
        }
        return valid;
    }
 
    bool
    KeypointsAdmittanceController::updateControlStatus(
        const Config& cfg,
        const IceUtil::Time& timeSinceLastIteration,
        std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors,
        std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors,
        std::vector<const SensorValue1DoFActuatorPosition*> positionSensors)
    {
        /// check size and value
        {
            ARMARX_CHECK_EQUAL(numOfJoints, torqueSensors.size());
            ARMARX_CHECK_EQUAL(numOfJoints, velocitySensors.size());
            ARMARX_CHECK_EQUAL(numOfJoints, positionSensors.size());
 
            ARMARX_CHECK_EQUAL(numOfJoints, cfg.kpNullspace.size());
            ARMARX_CHECK_EQUAL(numOfJoints, cfg.kdNullspace.size());
            ARMARX_CHECK_EQUAL(numOfJoints, cfg.desiredNullspaceJointAngles.size());
            ARMARX_CHECK_GREATER_EQUAL(cfg.torqueLimit, 0);
 
            ARMARX_CHECK_EQUAL(cfg.numPoints * 3, cfg.keypointKp.size());
            ARMARX_CHECK_EQUAL(cfg.numPoints * 3, cfg.keypointKd.size());
            ARMARX_CHECK_EQUAL(cfg.numPoints * 3, cfg.currentKeypointPosition.size());
            ARMARX_CHECK_EQUAL(cfg.numPoints * 3, cfg.desiredKeypointPosition.size());
            ARMARX_CHECK_EQUAL(cfg.numPoints * 3, cfg.desiredKeypointVelocity.size());
            ARMARX_CHECK_EQUAL(cfg.numPoints * 3, cfg.fixedTranslation.size());
        }
 
        /// ----------------------------- get target status from config ---------------------------------------------
        s.kpImpedance = cfg.kpImpedance;
        s.kdImpedance = cfg.kdImpedance;
        s.kpAdmittance = cfg.kpAdmittance;
        s.kdAdmittance = cfg.kdAdmittance;
        s.kmAdmittance = cfg.kmAdmittance;
        s.kpNullspace = cfg.kpNullspace;
        s.kdNullspace = cfg.kdNullspace;
 
        s.torqueLimit = cfg.torqueLimit;
        s.qvelFilter = cfg.qvelFilter;
 
        s.currentForceTorque = cfg.currentForceTorque;
        s.desiredNullspaceJointAngles = cfg.desiredNullspaceJointAngles;
 
        /// update keypoints related variables
        s.numPoints = cfg.numPoints;
        s.keypointKp = cfg.keypointKp;
        s.keypointKd = cfg.keypointKd;
 
        s.currentKeypointPosition = cfg.currentKeypointPosition;
        s.desiredKeypointPosition = cfg.desiredKeypointPosition;
        s.desiredKeypointVelocity = cfg.desiredKeypointVelocity;
        s.keypointPositionFilter = cfg.keypointPositionFilter;
        s.keypointVelocityFilter = cfg.keypointVelocityFilter;
 
        s.isRigid = cfg.isRigid;
        s.fixedTranslation = cfg.fixedTranslation;
 
        /// ----------------------------- 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;
 
        Eigen::VectorXf qvelRaw(numOfJoints);
        for (size_t i = 0; i < velocitySensors.size(); ++i)
        {
            s.qpos(i) = positionSensors[i]->position;
            qvelRaw(i) = velocitySensors[i]->velocity;
        }
 
        s.qvel = (1 - cfg.qvelFilter) * s.qvel + cfg.qvelFilter * qvelRaw;
        s.currentTwist = jacobi * s.qvel;
        s.deltaT = timeSinceLastIteration.toSecondsDouble();
 
        /// ----------------------------- keypoint related ---------------------------------------------
        /// compute (filtered) keypoint position
        if (s.isRigid)
        {
            for (int i = 0; i < s.numPoints; i++)
            {
                s.currentKeypointPosition.segment(3 * i, 3) =
                    s.currentPose.block(0, 0, 3, 3) * s.fixedTranslation.segment(3 * i, 3) +
                    s.currentPose.block<3, 1>(0, 3);
            }
        }
        ARMARX_CHECK_EQUAL(s.filteredKeypointPosition.size(), s.currentKeypointPosition.size());
        s.filteredKeypointPosition =
            (1.0f - s.keypointPositionFilter) * s.filteredKeypointPosition +
            s.keypointPositionFilter * s.currentKeypointPosition;
        /// compute filtered keypoint velocity
        if (s.isRigid)
        {
            Eigen::VectorXf currentKeypointVelocity;
            currentKeypointVelocity.setZero(s.numPoints * 3);
            for (int i = 0; i < s.numPoints; i++)
            {
                Eigen::Vector3f angular_vel = s.currentTwist.tail<3>();
                Eigen::Vector3f dist = s.fixedTranslation.segment(3 * i, 3);
                currentKeypointVelocity.segment(3 * i, 3) =
                    angular_vel.cross(dist) + s.currentTwist.head<3>();
            }
            s.currentKeypointVelocity =
                (1.0f - s.keypointVelocityFilter) * s.currentKeypointVelocity +
                s.keypointVelocityFilter * currentKeypointVelocity;
        }
        else
        {
            s.currentKeypointVelocity =
                (1.0f - s.keypointVelocityFilter) * s.currentKeypointVelocity +
                s.keypointVelocityFilter *
                    (s.filteredKeypointPosition - s.previousKeypointPosition) / s.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(s.keypointKp) -
                                        s.currentKeypointVelocity.cwiseProduct(s.keypointKd);
        if (trackingForce.size() != 3 * s.numPoints)
        {
            trackingForce.setZero(3 * s.numPoints);
        }
 
        s.pointTrackingForce.setZero();
        for (int i = 0; i < s.numPoints; i++)
        {
            Eigen::Vector3f dist =
                s.filteredKeypointPosition.segment(3 * i, 3) - s.currentPose.block<3, 1>(0, 3);
            dist = dist * 0.001;
            Eigen::Vector3f force = trackingForce.segment(3 * i, 3);
            if (force.norm() > 200)
            {
                ARMARX_RT_LOGF_WARN("force too large, set to zero");
                force.setZero();
            }
            s.pointTrackingForce.head<3>() += force;
            s.pointTrackingForce.tail<3>() += dist.cross(force);
        }
        ARMARX_CHECK_EQUAL(s.currentForceTorque.size(), s.pointTrackingForce.size());
        Eigen::Vector6f acc = s.kmAdmittance.cwiseProduct(s.pointTrackingForce) -
                              s.kdAdmittance.cwiseProduct(s.desiredVel);
 
        Eigen::Vector6f vel = s.desiredVel + 0.5 * s.deltaT * (acc + s.desiredAcc);
        Eigen::VectorXf deltaPose = 0.5 * s.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);
 
        return validateTargetPose(s.desiredPose);
    }
 
    void
    KeypointsAdmittanceController::run(bool rtReady,
                                       std::vector<ControlTarget1DoFActuatorTorque*> targets)
    {
        /// only when the ft sensor is calibrated can we allow the admittance controller
        /// to have virtual pose modulation out of force/torque information.
        if (!rtReady)
        {
            s.desiredPose = s.previousDesiredPose;
            s.desiredVel.setZero();
            s.kmAdmittance.setZero();
        }
        /// ---------------------------- admittance control ---------------------------------------------
        /// calculate pose error between the virtual pose and the target pose
        Eigen::Matrix3f objDiffMat =
            s.virtualPose.block<3, 3>(0, 0) * s.desiredPose.block<3, 3>(0, 0).transpose();
        Eigen::Vector6f poseError;
        poseError.head<3>() = s.virtualPose.block<3, 1>(0, 3) - s.desiredPose.block<3, 1>(0, 3);
        poseError.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(objDiffMat);
 
        /// admittance control law and Euler Integration -> virtual pose
        Eigen::Vector6f acc = s.kmAdmittance.cwiseProduct(s.currentForceTorque) -
                              s.kpAdmittance.cwiseProduct(poseError) -
                              s.kdAdmittance.cwiseProduct(s.virtualVel);
 
        Eigen::Vector6f vel = s.virtualVel + 0.5 * s.deltaT * (acc + s.virtualAcc);
        Eigen::VectorXf deltaPose = 0.5 * s.deltaT * (vel + s.virtualVel);
        s.virtualAcc = acc;
        s.virtualVel = vel;
 
        Eigen::Matrix3f deltaPoseMat =
            VirtualRobot::MathTools::rpy2eigen3f(deltaPose(3), deltaPose(4), deltaPose(5));
        s.virtualPose.block<3, 1>(0, 3) += deltaPose.head<3>();
        s.virtualPose.block<3, 3>(0, 0) = deltaPoseMat * s.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.
 
        Eigen::Matrix3f diffMat =
            s.virtualPose.block<3, 3>(0, 0) * s.currentPose.block<3, 3>(0, 0).transpose();
        Eigen::Vector6f poseErrorImp;
        poseErrorImp.head<3>() =
            0.001 * (s.virtualPose.block<3, 1>(0, 3) - s.currentPose.block<3, 1>(0, 3));
        poseErrorImp.tail<3>() = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
        s.forceImpedance =
            s.kpImpedance.cwiseProduct(poseErrorImp) - s.kdImpedance.cwiseProduct(s.currentTwist);
 
        /// ----------------------------- Nullspace PD Control --------------------------------------------------
        Eigen::VectorXf nullspaceTorque =
            s.kpNullspace.cwiseProduct(s.desiredNullspaceJointAngles - s.qpos) -
            s.kdNullspace.cwiseProduct(s.qvel);
 
        /// ----------------------------- Map TS target force to JS --------------------------------------------------
        const Eigen::MatrixXf jtpinv =
            ik->computePseudoInverseJacobianMatrix(s.jacobi.transpose(), lambda);
        s.desiredJointTorques = s.jacobi.transpose() * s.forceImpedance +
                                (I - s.jacobi.transpose() * jtpinv) * nullspaceTorque;
 
        ARMARX_CHECK_EQUAL(targets.size(), (unsigned)s.desiredJointTorques.size());
        for (size_t i = 0; i < targets.size(); ++i)
        {
            s.desiredJointTorques(i) =
                boost::algorithm::clamp(s.desiredJointTorques(i), -s.torqueLimit, s.torqueLimit);
            targets.at(i)->torque = s.desiredJointTorques(i);
            if (!targets.at(i)->isValid())
            {
                targets.at(i)->torque = 0;
            }
        }
    }
} // namespace armarx::control::common::control_law