Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
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
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
00053 X[i]=chain.getSegment(i).pose(q_);
00054
00055
00056
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
00060
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
00069
00070 RigidBodyInertia Ii=chain.getSegment(i).getInertia();
00071 f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
00072
00073 }
00074
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 }