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 "chaindynparam.hpp"
00023 #include "frames_io.hpp"
00024 #include <iostream>
00025
00026 namespace KDL {
00027
00028 ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav):
00029 chain(_chain),
00030 nr(0),
00031 nj(chain.getNrOfJoints()),
00032 ns(chain.getNrOfSegments()),
00033 grav(_grav),
00034 jntarraynull(nj),
00035 chainidsolver_coriolis( chain, Vector::Zero()),
00036 chainidsolver_gravity( chain, grav),
00037 wrenchnull(ns,Wrench::Zero()),
00038 X(ns),
00039 S(ns),
00040 Ic(ns)
00041 {
00042 ag=-Twist(grav,Vector::Zero());
00043 }
00044
00045
00046 int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H)
00047 {
00048
00049 if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj )
00050 return -1;
00051 unsigned int k=0;
00052 double q_;
00053
00054
00055 for(unsigned int i=0;i<ns;i++)
00056 {
00057
00058 Ic[i]=chain.getSegment(i).getInertia();
00059 if(chain.getSegment(i).getJoint().getType()!=Joint::None)
00060 {
00061 q_=q(k);
00062 k++;
00063 }
00064 else
00065 {
00066 q_=0.0;
00067 }
00068 X[i]=chain.getSegment(i).pose(q_);
00069 S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
00070 }
00071
00072 int j,l;
00073 k=nj-1;
00074 for(int i=ns-1;i>=0;i--)
00075 {
00076
00077 if(i!=0)
00078 {
00079
00080 Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
00081 }
00082
00083 F=Ic[i]*S[i];
00084 if(chain.getSegment(i).getJoint().getType()!=Joint::None)
00085 {
00086 H(k,k)=dot(S[i],F);
00087 j=k;
00088 l=i;
00089 while(l!=0)
00090 {
00091
00092 F=X[l]*F;
00093 l--;
00094
00095 if(chain.getSegment(l).getJoint().getType()!=Joint::None)
00096 {
00097 j--;
00098 H(k,j)=dot(F,S[l]);
00099 H(j,k)=H(k,j);
00100 }
00101 }
00102 k--;
00103 }
00104
00105 }
00106 return 0;
00107 }
00108
00109
00110 int ChainDynParam::JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis)
00111 {
00112
00113 SetToZero(jntarraynull);
00114
00115
00116
00117 chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis);
00118
00119 return 0;
00120 }
00121
00122
00123 int ChainDynParam::JntToGravity(const JntArray &q,JntArray &gravity)
00124 {
00125
00126
00127
00128 SetToZero(jntarraynull);
00129
00130 chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity);
00131 return 0;
00132 }
00133
00134 ChainDynParam::~ChainDynParam()
00135 {
00136 }
00137
00138
00139 }