KdlChainIdInertia.cpp

Go to the documentation of this file.

#include <kdl/frames_io.hpp>

#include "robodyn_controllers/KdlChainIdInertia.h"

using namespace KDL;

/***************************************************************************/
KdlChainIdInertia::KdlChainIdInertia(const Chain& chain_, Vector grav):
    chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()),
    X(ns), S(ns), v(ns), a(ns), f(ns), Ii(ns)
{
    ag = -Twist(grav, Vector::Zero());
}
/***************************************************************************/
int KdlChainIdInertia::CartToJnt(const JntArray& q, const JntArray& q_dot, const JntArray& q_dotdot, const Wrenches& forceExt, JntArray& torques, JntArray& Hv)
{
    //Check sizes when in debug mode
    if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj || forceExt.size() != ns || Hv.rows() != nj )
    {
        return -1;
    }

    unsigned int j = 0;

    //Sweep from root to leaf
    for (unsigned int i = 0; i < ns; i++)
    {
        double q_, qdot_, qdotdot_;

        if (chain.getSegment(i).getJoint().getType() != Joint::None)
        {
            q_ = q(j);
            qdot_ = q_dot(j);
            qdotdot_ = q_dotdot(j);
            j++;
        }
        else
        {
            q_ = qdot_ = qdotdot_ = 0.0;
        }

        //Calculate segment properties: X,S,vj,cj
        X[i] = chain.getSegment(i).pose(q_);//Remark this is the inverse of the
        //frame for transformations from
        //the parent to the current coord frame
        //Transform velocity and unit velocity to segment frame
        Twist vj = X[i].M.Inverse(chain.getSegment(i).twist(q_, qdot_));
        S[i]     = X[i].M.Inverse(chain.getSegment(i).twist(q_, 1.0));

        //We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
        //calculate velocity and acceleration of the segment (in segment coordinates)
        if (i == 0)
        {
            v[i] = vj;
            a[i] = X[i].Inverse(ag) + S[i] * qdotdot_ + v[i] * vj;
        }
        else
        {
            v[i] = X[i].Inverse(v[i - 1]) + vj;
            a[i] = X[i].Inverse(a[i - 1]) + S[i] * qdotdot_ + v[i] * vj;
        }

        //Calculate the force for the joint
        //Collect RigidBodyInertia and external forces
        Ii[i] = chain.getSegment(i).getInertia();
        f[i]  = Ii[i] * a[i] + v[i] * (Ii[i] * v[i]) - forceExt[i];
    }

    //Sweep from leaf to root
    j = nj - 1;

    for (int i = ns - 1; i >= 0; i--)
    {
        if (i != 0)
        {
            f[i - 1]  = f[i - 1] +  X[i] * f[i];
            Ii[i - 1] = Ii[i - 1] + X[i] * Ii[i];
        }

        Fis = Ii[i] * S[i];

        if (chain.getSegment(i).getJoint().getType() != Joint::None)
        {
            torques(j) = dot(S[i], f[i]);
            Hv(j--)    = dot(S[i], Fis);
        }
    }

    return 0;
}