Пример #1
0
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;
}