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 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
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
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
00066 X[i]=chain.getSegment(i).pose(q_);
00067
00068
00069
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
00073
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
00082
00083 RigidBodyInertia Ii=chain.getSegment(i).getInertia();
00084 f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
00085
00086 }
00087
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);
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 }