DynamicsUtilities.cpp

Go to the documentation of this file.

#include "robodyn_controllers/DynamicsUtilities.h"
#include <iostream>

using namespace Eigen;
/***************************************************************************/
DynamicsUtilities::DynamicsUtilities()
{
}

DynamicsUtilities::~DynamicsUtilities()
{
}
/***************************************************************************/
bool DynamicsUtilities::isBaseFrame(const std::vector<std::string> baseFrames, const std::string frame)
{
    for (unsigned int i = 0; i < baseFrames.size(); i++)
    {
        if (frame == baseFrames[i])
        {
            return true;
        }
    }

    return false;
}
/***************************************************************************/
void DynamicsUtilities::MultiplyJacobianTranspose(const KDL::Jacobian& jac, const KDL::Wrench& src, KDL::JntArray& dest)
{
    Eigen::Matrix<double, 6, 1> w;
    w(0) = src.force(0);
    w(1) = src.force(1);
    w(2) = src.force(2);
    w(3) = src.torque(0);
    w(4) = src.torque(1);
    w(5) = src.torque(2);

    Eigen::MatrixXd j = jac.data;
    j.transposeInPlace();

    Eigen::VectorXd t(jac.columns());
    t = j * w;

    dest.resize(jac.columns());

    for (unsigned int i = 0; i < jac.columns(); i++)
    {
        dest(i) = t(i);
    }
}
/***************************************************************************/
int DynamicsUtilities::MultiplyJacobianTransposeInverse(const KDL::Jacobian& jac, const KDL::JntArray& src, KDL::Wrench& dest)
{

    unsigned int nj          = jac.columns();
    unsigned int nc          = jac.rows();  // should be 6
    int          critMaxIter = 100;
    double       eps         = 0.1;

    Eigen::MatrixXd jt = jac.data;
    jt.transposeInPlace();

    if (src.rows() != nj)
    {
        return -1;
    }

    Eigen::VectorXd w(nj);
    Eigen::VectorXd out(nc);

    for (unsigned int j = 0; j < nj; j++)
    {
        w(j) = src(j);
    }

    // get SVD
    VectorXd tmp(nc);
    VectorXd S(VectorXd::Zero(nc));
    MatrixXd U(MatrixXd::Identity(nj, nc));
    MatrixXd V(MatrixXd::Identity(nc, nc));

    if (KDL::svd_eigen_HH(jt, U, S, V, tmp, critMaxIter) < 0)
    {
        std::cerr << "SVD calculation failed" << std::endl;
        return -1;
    }

    //first we calculate Ut*v_in
    double sum;

    for (unsigned int i = 0; i < nc; ++i)
    {
        sum = 0.0;

        for (unsigned int j = 0; j < nj; ++j)
        {
            sum += U(j, i) * w(j);
        }

        // truncated PseudoInverse
        tmp[i] = sum * (fabs(S[i]) < eps ? 0.0 : 1.0 / S[i]);
    }

    //tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
    //it with V to get cart force at ee
    for (unsigned int i = 0; i < nc; ++i)
    {
        sum = 0.0;

        for (unsigned int j = 0; j < nc; ++j)
        {
            sum += V(i, j) * tmp[j];
        }

        //Put the result in out vector
        out(i) = sum;
    }

    dest.force.x(out(0));
    dest.force.y(out(1));
    dest.force.z(out(2));
    dest.torque.x(out(3));
    dest.torque.y(out(4));
    dest.torque.z(out(5));

    return 0;
}

double DynamicsUtilities::mag(const KDL::Vector& vec)
{
    double val = 0.0;

    for (unsigned int i = 0; i < 3; i++)
    {
        val += vec(i) * vec(i);
    }

    return sqrt(val);
}
/***************************************************************************/
KDL::Wrench DynamicsUtilities::SumWrenches(const std::vector<KDL::Wrench>& forceCenter)
{
    KDL::Wrench forceOut;

    for (unsigned int i = 0; i < forceCenter.size(); i++)
    {
        forceOut += forceCenter[i];
    }

    return forceOut;
}