chainidsolver_recursive_newton_euler.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2009  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 #include "chainidsolver_recursive_newton_euler.hpp"
00023 #include "frames_io.hpp"
00024 
00025 namespace KDL{
00026     
00027     ChainIdSolver_RNE::ChainIdSolver_RNE(const Chain& chain_,Vector grav):
00028         chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()),
00029         X(ns),S(ns),v(ns),a(ns),f(ns)
00030     {
00031         ag=-Twist(grav,Vector::Zero());
00032     }
00033 
00034     int ChainIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)
00035     {
00036         //Check sizes when in debug mode
00037         if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
00038             return (error = -1);
00039         unsigned int j=0;
00040 
00041         //Sweep from root to leaf
00042         for(unsigned int i=0;i<ns;i++){
00043             double q_,qdot_,qdotdot_;
00044             if(chain.getSegment(i).getJoint().getType()!=Joint::None){
00045                 q_=q(j);
00046                 qdot_=q_dot(j);
00047                 qdotdot_=q_dotdot(j);
00048                 j++;
00049             }else
00050                 q_=qdot_=qdotdot_=0.0;
00051             
00052             //Calculate segment properties: X,S,vj,cj
00053             X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the 
00054                                                 //frame for transformations from 
00055                                                 //the parent to the current coord frame
00056             //Transform velocity and unit velocity to segment frame
00057             Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));
00058             S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
00059             //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
00060             //calculate velocity and acceleration of the segment (in segment coordinates)
00061             if(i==0){
00062                 v[i]=vj;
00063                 a[i]=X[i].Inverse(ag)+S[i]*qdotdot_+v[i]*vj;
00064             }else{
00065                 v[i]=X[i].Inverse(v[i-1])+vj;
00066                 a[i]=X[i].Inverse(a[i-1])+S[i]*qdotdot_+v[i]*vj;
00067             }
00068             //Calculate the force for the joint
00069             //Collect RigidBodyInertia and external forces
00070             RigidBodyInertia Ii=chain.getSegment(i).getInertia();
00071             f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
00072             //std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
00073         }
00074         //Sweep from leaf to root
00075         j=nj-1;
00076         for(int i=ns-1;i>=0;i--){
00077             if(chain.getSegment(i).getJoint().getType()!=Joint::None)
00078                 torques(j--)=dot(S[i],f[i]);
00079             if(i!=0)
00080                 f[i-1]=f[i-1]+X[i]*f[i];
00081         }
00082         return (error = E_NOERROR);
00083     }
00084 }//namespace


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14