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:
parent
efb08237ac
commit
cd0ae20ef5
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue
Block a user