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

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

@ -26,7 +26,7 @@
namespace KDL {
ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav):
chain(_chain),
chain(_chain),
grav(_grav),
chainidsolver_coriolis( chain, Vector::Zero()),
chainidsolver_gravity( chain, grav),
@ -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_;
//Sweep from root to leaf
double q_;
//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];
}
{
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(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
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
@ -110,10 +111,10 @@ namespace KDL {
//make a null matrix with the size of q_dotdot and a null wrench
SetToZero(jntarraynull);
//the calculation of coriolis matrix C
return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis);
}
@ -122,7 +123,7 @@ namespace KDL {
{
//make a null matrix with the size of q_dotdot and a null wrench
SetToZero(jntarraynull);
//the calculation of coriolis matrix C
return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity);

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

@ -23,7 +23,7 @@
#include "frames_io.hpp"
namespace KDL{
ChainIdSolver_RNE::ChainIdSolver_RNE(const Chain& chain_,Vector grav):
chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()),
X(ns),S(ns),v(ns),a(ns),f(ns)
@ -48,10 +48,10 @@ namespace KDL{
j++;
}else
q_=qdot_=qdotdot_=0.0;
//Calculate segment properties: X,S,vj,cj
X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the
//frame for transformations from
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
//Transform velocity and unit velocity to segment frame
Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));
@ -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