CartesianImpedanceController.cpp

Go to the documentation of this file.

 
#include "CartesianImpedanceController.h"
 
#include <algorithm>
 
#include <SimoxUtility/math/compare/is_equal.h>
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
#include <VirtualRobot/MathTools.h>
 
 
using namespace armarx;
 
int
CartesianImpedanceController::bufferSize() const
{
    return targetJointTorques.size();
}
 
void
CartesianImpedanceController::initialize(const VirtualRobot::RobotNodeSetPtr& rns)
{
    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));
 
    const auto size = rns->getSize();
    qpos.resize(size);
    qvel.resize(size);
    tcptwist.resize(size);
    nullqerror.resize(size);
    nullspaceTorque.resize(size);
    targetJointTorques.resize(size);
 
    I = Eigen::MatrixXf::Identity(size, size);
}
 
const Eigen::VectorXf&
CartesianImpedanceController::run(
    const Config& cfg,
    std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors,
    std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors,
    std::vector<const SensorValue1DoFActuatorPosition*> positionSensors)
{
    ARMARX_CHECK_EXPRESSION(
        simox::math::is_equal(bufferSize(), cfg.Knull.size())); ///TODO move this to core
    ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.Dnull.size()));
    ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), cfg.desiredJointPosition.size()));
    ARMARX_CHECK_GREATER_EQUAL(cfg.torqueLimit, 0);
 
    ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), torqueSensors.size()));
    ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), velocitySensors.size()));
    ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), positionSensors.size()));
 
    const Eigen::MatrixXf jacobi = ik->getJacobianMatrix(
        tcp, VirtualRobot::IKSolver::CartesianSelection::All); ///TODO output param version
    ARMARX_CHECK_EXPRESSION(simox::math::is_equal(bufferSize(), jacobi.cols()));
    ARMARX_CHECK_EXPRESSION(simox::math::is_equal(6, jacobi.rows()));
    const Eigen::MatrixXf jtpinv = ik->computePseudoInverseJacobianMatrix(
        jacobi.transpose(), lambda); ///TODO output param version
 
    for (size_t i = 0; i < velocitySensors.size(); ++i)
    {
        qpos(i) = positionSensors[i]->position;
        qvel(i) = velocitySensors[i]->velocity;
    }
 
    const Eigen::Vector6f tcptwist = jacobi * qvel;
 
    const Eigen::Vector3f currentTCPLinearVelocity{
        0.001f * tcptwist(0), 0.001f * tcptwist(1), 0.001f * tcptwist(2)};
    const Eigen::Vector3f currentTCPAngularVelocity{tcptwist(3), tcptwist(4), tcptwist(5)};
 
    const Eigen::Vector3f currentTCPPosition = tcp->getPositionInRootFrame();
    const Eigen::Vector3f tcpForces =
        0.001 * cfg.Kpos.cwiseProduct(cfg.desiredPosition - currentTCPPosition);
    const Eigen::Vector3f tcpDesiredForce =
        tcpForces - cfg.Dpos.cwiseProduct(currentTCPLinearVelocity);
 
    const Eigen::Matrix4f currentTCPPose = tcp->getPoseInRootFrame();
    const Eigen::Matrix3f currentRotMat = currentTCPPose.block<3, 3>(0, 0);
    const Eigen::Matrix3f desiredMat(cfg.desiredOrientation);
    const Eigen::Matrix3f diffMat = desiredMat * currentRotMat.inverse();
    //    const Eigen::Quaternionf cquat(currentRotMat);
    //    const Eigen::Quaternionf diffQuaternion = desiredQuaternion * cquat.conjugate();
    //    const Eigen::AngleAxisf angleAxis(diffQuaternion);
 
    const Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen3f2rpy(diffMat);
    const Eigen::Vector3f tcpDesiredTorque =
        cfg.Kori.cwiseProduct(rpy) - cfg.Dori.cwiseProduct(currentTCPAngularVelocity);
    Eigen::Vector6f tcpDesiredWrench;
    tcpDesiredWrench << 0.001 * tcpDesiredForce, tcpDesiredTorque;
 
    // calculate desired joint torque
    nullqerror = cfg.desiredJointPosition - qpos;
 
    for (int i = 0; i < nullqerror.rows(); ++i)
    {
        if (fabs(nullqerror(i)) < 0.09)
        {
            nullqerror(i) = 0;
        }
    }
    nullspaceTorque = cfg.Knull.cwiseProduct(nullqerror) - cfg.Dnull.cwiseProduct(qvel);
 
    targetJointTorques =
        jacobi.transpose() * tcpDesiredWrench + (I - jacobi.transpose() * jtpinv) * nullspaceTorque;
    for (int i = 0; i < targetJointTorques.size(); ++i)
    {
        targetJointTorques(i) =
            std::clamp(targetJointTorques(i), -cfg.torqueLimit, cfg.torqueLimit);
    }
    //write debug data
    {
        const Eigen::Quaternionf cquat(currentRotMat);
        dbg.errorAngle = cquat.angularDistance(cfg.desiredOrientation);
        dbg.posiError = (cfg.desiredPosition - currentTCPPosition).norm();
        dbg.tcpDesiredForce = tcpDesiredForce;
        dbg.tcpDesiredTorque = tcpDesiredTorque;
    }
 
    return targetJointTorques;
}