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     void ChainIdSolver_RNE::updateInternalDataStructures() {
00035         nj = chain.getNrOfJoints();
00036         ns = chain.getNrOfSegments();
00037         X.resize(ns);
00038         S.resize(ns);
00039         v.resize(ns);
00040         a.resize(ns);
00041         f.resize(ns);
00042     }
00043 
00044     int ChainIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)
00045     {
00046         if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments())
00047             return (error = E_NOT_UP_TO_DATE);
00048 
00049         //Check sizes when in debug mode
00050         if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
00051             return (error = E_SIZE_MISMATCH);
00052         unsigned int j=0;
00053 
00054         //Sweep from root to leaf
00055         for(unsigned int i=0;i<ns;i++){
00056             double q_,qdot_,qdotdot_;
00057             if(chain.getSegment(i).getJoint().getType()!=Joint::None){
00058                 q_=q(j);
00059                 qdot_=q_dot(j);
00060                 qdotdot_=q_dotdot(j);
00061                 j++;
00062             }else
00063                 q_=qdot_=qdotdot_=0.0;
00064             
00065             //Calculate segment properties: X,S,vj,cj
00066             X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the 
00067                                                 //frame for transformations from 
00068                                                 //the parent to the current coord frame
00069             //Transform velocity and unit velocity to segment frame
00070             Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));
00071             S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
00072             //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
00073             //calculate velocity and acceleration of the segment (in segment coordinates)
00074             if(i==0){
00075                 v[i]=vj;
00076                 a[i]=X[i].Inverse(ag)+S[i]*qdotdot_+v[i]*vj;
00077             }else{
00078                 v[i]=X[i].Inverse(v[i-1])+vj;
00079                 a[i]=X[i].Inverse(a[i-1])+S[i]*qdotdot_+v[i]*vj;
00080             }
00081             //Calculate the force for the joint
00082             //Collect RigidBodyInertia and external forces
00083             RigidBodyInertia Ii=chain.getSegment(i).getInertia();
00084             f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
00085             //std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
00086         }
00087         //Sweep from leaf to root
00088         j=nj-1;
00089         for(int i=ns-1;i>=0;i--){
00090             if(chain.getSegment(i).getJoint().getType()!=Joint::None){
00091                 torques(j)=dot(S[i],f[i]);
00092                 torques(j)+=chain.getSegment(i).getJoint().getInertia()*q_dotdot(j);  // add torque from joint inertia
00093                 --j;
00094             }
00095             if(i!=0)
00096                 f[i-1]=f[i-1]+X[i]*f[i];
00097         }
00098         return (error = E_NOERROR);
00099     }
00100 }//namespace


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:22