Skip to content

Commit

Permalink
Mod/Robot: fix some spaces and a couple of warnings
Browse files Browse the repository at this point in the history 
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]
  • Loading branch information
@Fat-Zer @wwmayer
Fat-Zer authored and wwmayer committed Aug 29, 2015
1 parent efb0823 commit cd0ae20
Show file tree
Hide file tree
Showing 2 changed files with 60 additions and 58 deletions.
107 changes: 54 additions & 53 deletions src/Mod/Robot/App/kdl_cp/chaindynparam.cpp
View file
Expand Up @@ -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),
Expand All @@ -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
Expand All @@ -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);


}

Expand All @@ -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);
Expand Down
11 changes: 6 additions & 5 deletions src/Mod/Robot/App/kdl_cp/chainidsolver_recursive_newton_euler.cpp
View file
Expand Up @@ -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)
Expand All @@ -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_));
Expand All @@ -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;
Expand All @@ -79,5 +79,6 @@ namespace KDL{
if(i!=0)
f[i-1]=f[i-1]+X[i]*f[i];
}
return 0;
}
}//namespace

0 comments on commit cd0ae20

Please sign in to comment.