bool checkTransformAreEqual(const iDynTree::Transform & t1,
const iDynTree::Transform & t2,
const double tol)
{
return checkMatrixAreEqual(t1.getRotation(),t2.getRotation(),tol) &&
checkVectorAreEqual(t1.getPosition(),t2.getPosition(),tol);
}
bool checkFrameConsistency(KDL::Frame & frame_kdl,
iDynTree::Transform & trans_idyntree,
double tol = 1e-9
)
{
iDynTree::Rotation rot_idyn = trans_idyntree.getRotation();
KDL::Rotation rot_kdl = frame_kdl.M;
for(int row = 0; row < 3; row++)
{
for(int col = 0; col < 3; col++)
{
if( fabs(rot_idyn(row,col)-rot_kdl(row,col)) > tol )
{
return false;
}
}
}
iDynTree::Position pos_idyn = trans_idyntree.getPosition();
KDL::Vector pos_kdl = frame_kdl.p;
for(int row = 0; row < 3; row++)
{
if( fabs(pos_idyn(row)-pos_kdl(row)) > tol )
{
return false;
}
}
return true;
}
