chaindynparam.cpp

Go to the documentation of this file.

// Copyright  (C)  2009  Dominick Vanthienen <dominick dot vanthienen at intermodalics dot eu>

// Version: 1.0
// Author: Dominick Vanthienen <dominick dot vanthienen at intermodalics dot eu>
// Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#include "chaindynparam.hpp"
#include "frames_io.hpp"
#include <iostream>

namespace KDL {

    ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav):
            chain(_chain),
            nr(0),
            nj(chain.getNrOfJoints()),
            ns(chain.getNrOfSegments()),
            grav(_grav),
            jntarraynull(nj),
            chainidsolver_coriolis( chain, Vector::Zero()),
            chainidsolver_gravity( chain, grav),
            wrenchnull(ns,Wrench::Zero()),
            X(ns),
            S(ns),
            Ic(ns)
    {
        ag=-Twist(grav,Vector::Zero());
    }

    void ChainDynParam::updateInternalDataStructures() {
        nj = chain.getNrOfJoints();
        ns = chain.getNrOfSegments();
        jntarraynull.resize(nj);
        chainidsolver_coriolis.updateInternalDataStructures();
        chainidsolver_gravity.updateInternalDataStructures();
        wrenchnull.resize(ns,Wrench::Zero());
        X.resize(ns);
        S.resize(ns);
        Ic.resize(ns);
    }


    //calculate inertia matrix H
    int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H)
    {
        if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments())
            return (error = E_NOT_UP_TO_DATE);
        //Check sizes when in debug mode
        if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj )
            return (error = E_SIZE_MISMATCH);
        unsigned int k=0;
        double q_;

        //Sweep from root to leaf
        for(unsigned int i=0;i<ns;i++)
        {
          //Collect RigidBodyInertia
          Ic[i]=chain.getSegment(i).getInertia();
          if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed)
          {
              q_=q(k);
              k++;
          }
          else
          {
            q_=0.0;
          }
          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
          S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
        }
        //Sweep from leaf to root
        int j,l;
        k=nj-1; //reset k
        for(int i=ns-1;i>=0;i--)
        {

          if(i!=0)
            {
              //assumption that previous segment is parent
              Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
            }

          F=Ic[i]*S[i];
      if(chain.getSegment(i).getJoint().getType()!=Joint::Fixed)
          {
          H(k,k)=dot(S[i],F);
          H(k,k)+=chain.getSegment(i).getJoint().getInertia();  // add joint inertia
              j=k; //countervariable for the joints
              l=i; //countervariable for the segments
              while(l!=0) //go from leaf to root starting at i
                {
                  //assumption that previous segment is parent
                  F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l]
                  l--; //go down a segment

          if(chain.getSegment(l).getJoint().getType()!=Joint::Fixed) //if the joint connected to segment is not a fixed joint
                  {
                    j--;
                    H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment
                    H(j,k)=H(k,j);
                  }
                }
              k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
          }

        }
        return (error = E_NOERROR);
    }

    //calculate coriolis matrix C
    int ChainDynParam::JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis)
    {
    //make a null matrix with the size of q_dotdot and a null wrench
        SetToZero(jntarraynull);


        //the calculation of coriolis matrix C
        return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis);

    }

    //calculate gravity matrix G
    int ChainDynParam::JntToGravity(const JntArray &q,JntArray &gravity)
    {

        //make a null matrix with the size of q_dotdot and a null wrench

        SetToZero(jntarraynull);
        //the calculation of coriolis matrix C
        return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity);
    }

    ChainDynParam::~ChainDynParam()
    {
    }


}