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