suzanne.soy/FreeCAD
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
src/Mod/Robot/App/kdl_cp/chaindynparam.cpp
View File

@ -44,64 +44,65 @@ namespace KDL {
//calculate inertia matrix 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 )
return -1;
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++)
{
//Collect RigidBodyInertia
{
//Collect RigidBodyInertia
Ic[i]=chain.getSegment(i).getInertia();
if(chain.getSegment(i).getJoint().getType()!=Joint::None)
{
q_=q(k);
k++;
}
else
{
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
S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
{
q_=q(k);
k++;
}
else
{
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
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;
k=nj-1; //reset k
k=nj-1; //reset k
for(int i=ns-1;i>=0;i--)
{
{
if(i!=0)
{
//assumption that previous segment is parent
Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
}
if(i!=0)
{
//assumption that previous segment is parent
Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
}
F=Ic[i]*S[i];
if(chain.getSegment(i).getJoint().getType()!=Joint::None)
{
H(k,k)=dot(S[i],F);
j=k; //countervariable for the joints
l=i; //countervariable for the segments
while(l!=0) //go from leaf to root starting at i
{
//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]
l--; //go down a segment
F=Ic[i]*S[i];
if(chain.getSegment(i).getJoint().getType()!=Joint::None)
{
H(k,k)=dot(S[i],F);
j=k; //countervariable for the joints
l=i; //countervariable for the segments
while(l!=0) //go from leaf to root starting at i
{
//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]
l--; //go down a segment
if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint
{
j--;
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);
}
}
k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
}
if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint
{
j--;
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);
}
}
k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
}
}
} // for
return 0;
}
//calculate coriolis matrix C

3
src/Mod/Robot/App/kdl_cp/chainidsolver_recursive_newton_euler.cpp
View File

@ -69,7 +69,7 @@ namespace KDL{
//Collect RigidBodyInertia and external forces
RigidBodyInertia Ii=chain.getSegment(i).getInertia();
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
j=nj-1;
@ -79,5 +79,6 @@ namespace KDL{
if(i!=0)
f[i-1]=f[i-1]+X[i]*f[i];
}
return 0;
}
}//namespace