Mod/Robot: fix some spaces and a couple of warnings

Warnings wsa:
src/Mod/Robot/App/kdl_cp/chaindynparam.cpp|105 col 5| warning: control may reach end of non-void function [-Wreturn-type]
src/Mod/Robot/App/kdl_cp/chainidsolver_recursive_newton_euler.cpp|82 col 5| warning: control may reach end of non-void function [-Wreturn-type]
This commit is contained in:
Alexander Golubev 2015-05-17 20:41:35 +03:00 committed by wmayer
parent efb08237ac
commit cd0ae20ef5
2 changed files with 58 additions and 56 deletions

View File

@ -44,64 +44,65 @@ namespace KDL {
//calculate inertia matrix H //calculate inertia matrix H
int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H) int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H)
{ {
//Check sizes when in debug mode //Check sizes when in debug mode
if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj ) if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj )
return -1; return -1;
unsigned int k=0; unsigned int k=0;
double q_; double q_;
//Sweep from root to leaf //Sweep from root to leaf
for(unsigned int i=0;i<ns;i++) for(unsigned int i=0;i<ns;i++)
{ {
//Collect RigidBodyInertia //Collect RigidBodyInertia
Ic[i]=chain.getSegment(i).getInertia(); Ic[i]=chain.getSegment(i).getInertia();
if(chain.getSegment(i).getJoint().getType()!=Joint::None) if(chain.getSegment(i).getJoint().getType()!=Joint::None)
{ {
q_=q(k); q_=q(k);
k++; k++;
} }
else else
{ {
q_=0.0; q_=0.0;
} }
X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the frame for transformations from the parent to the current coord frame X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the frame for transformations from the parent to the current coord frame
S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0)); S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
} }
//Sweep from leaf to root //Sweep from leaf to root
int j,l; int j,l;
k=nj-1; //reset k k=nj-1; //reset k
for(int i=ns-1;i>=0;i--) for(int i=ns-1;i>=0;i--)
{ {
if(i!=0) if(i!=0)
{ {
//assumption that previous segment is parent //assumption that previous segment is parent
Ic[i-1]=Ic[i-1]+X[i]*Ic[i]; Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
} }
F=Ic[i]*S[i]; F=Ic[i]*S[i];
if(chain.getSegment(i).getJoint().getType()!=Joint::None) if(chain.getSegment(i).getJoint().getType()!=Joint::None)
{ {
H(k,k)=dot(S[i],F); H(k,k)=dot(S[i],F);
j=k; //countervariable for the joints j=k; //countervariable for the joints
l=i; //countervariable for the segments l=i; //countervariable for the segments
while(l!=0) //go from leaf to root starting at i while(l!=0) //go from leaf to root starting at i
{ {
//assumption that previous segment is parent //assumption that previous segment is parent
F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l] F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l]
l--; //go down a segment l--; //go down a segment
if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint
{ {
j--; j--;
H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment
H(j,k)=H(k,j); H(j,k)=H(k,j);
} }
} }
k--; //this if-loop should be repeated nj times (k=nj-1 to k=0) k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
} }
} } // for
return 0;
} }
//calculate coriolis matrix C //calculate coriolis matrix C

View File

@ -69,7 +69,7 @@ namespace KDL{
//Collect RigidBodyInertia and external forces //Collect RigidBodyInertia and external forces
RigidBodyInertia Ii=chain.getSegment(i).getInertia(); RigidBodyInertia Ii=chain.getSegment(i).getInertia();
f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i]; f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
//std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl; //std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
} }
//Sweep from leaf to root //Sweep from leaf to root
j=nj-1; j=nj-1;
@ -79,5 +79,6 @@ namespace KDL{
if(i!=0) if(i!=0)
f[i-1]=f[i-1]+X[i]*f[i]; f[i-1]=f[i-1]+X[i]*f[i];
} }
return 0;
} }
}//namespace }//namespace