KeypointsAdmittanceController.cpp

Go to the documentation of this file.

#include <fstream>
#include <boost/algorithm/clamp.hpp>
 
#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 <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/json.h>
 
#include <VirtualRobot/MathTools.h>
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include "../utils.h"
#include "KeypointsAdmittanceController.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