[yarp] Added failing test on KDLtoYarp_twist and KDLtoYarp_wrench met…

…hods

src/icub/tests/AdjointMatricesConsistencyTest.cpp

@@ -0,0 +1,64 @@
+/*
+* Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia
+* Author: Silvio Traversaro <silvio.traversaro@iit.it>
+*
+* CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+*/
+
+#include <iDynTree/Core/MatrixFixSize.h>
+
+#include <iDynTree/Core/TestUtils.h>
+#include <iDynTree/Core/EigenHelpers.h>
+
+#include <kdl_codyco/KDLConversions.h>
+#include <kdl_codyco/regressor_utils.hpp>
+
+#include <iCub/iDynTree/yarp_kdl.h>
+
+#include <iDynTree/Core/Transform.h>
+
+#include <yarp/sig/Matrix.h>
+
+using namespace iDynTree;
+
+int main()
+{
+    // Create a random iDynTree frame
+    Transform trans = getRandomTransform();
+
+    Matrix6x6 adj_idyntree = trans.asAdjointTransform();
+    yarp::sig::Matrix adj_yarp_kdl_inyarp = KDLtoYarp_twist(ToKDL(trans));
+    Matrix6x6 adj_yarp_kdl;
+    toEigen(adj_yarp_kdl) = Eigen::Map<Eigen::Matrix<double,6,6,Eigen::RowMajor> >(adj_yarp_kdl_inyarp.data());
+    Matrix6x6 adj_eigen_kdl;
+    toEigen(adj_eigen_kdl) = KDL::CoDyCo::TwistTransformationMatrix(ToKDL(trans));
+
+    std::cerr << "Adjoint matrix: "<< std::endl;
+    std::cerr << "iDynTree : \n" << adj_idyntree.toString() << std::endl;
+    std::cerr << "YARP     : \n" << adj_yarp_kdl.toString() << std::endl;
+    std::cerr << "Eigen    : \n" << adj_eigen_kdl.toString() << std::endl;
+
+    Matrix6x6 adj_wrench_idyntree = trans.asAdjointTransformWrench();
+    yarp::sig::Matrix adj_wrench_yarp_kdl_inyarp = KDLtoYarp_wrench(ToKDL(trans));
+    Matrix6x6 adj_wrench_yarp_kdl;
+    toEigen(adj_wrench_yarp_kdl) = Eigen::Map<Eigen::Matrix<double,6,6,Eigen::RowMajor> >(adj_wrench_yarp_kdl_inyarp.data());
+
+    Matrix6x6 adj_wrench_eigen_kdl;
+    toEigen(adj_wrench_eigen_kdl) = KDL::CoDyCo::WrenchTransformationMatrix(ToKDL(trans));
+
+    std::cerr << "Wrench Adjoint matrix: "<< std::endl;
+    std::cerr << "iDynTree : \n" << adj_wrench_idyntree.toString() << std::endl;
+    std::cerr << "YARP     : \n" << adj_wrench_yarp_kdl.toString() << std::endl;
+    std::cerr << "Eigen    : \n" << adj_wrench_eigen_kdl.toString() << std::endl;
+
+    ASSERT_EQUAL_MATRIX(adj_idyntree,adj_yarp_kdl);
+    ASSERT_EQUAL_MATRIX(adj_idyntree,adj_eigen_kdl);
+
+    ASSERT_EQUAL_MATRIX(adj_wrench_idyntree,adj_wrench_yarp_kdl);
+    ASSERT_EQUAL_MATRIX(adj_wrench_idyntree,adj_wrench_eigen_kdl);
+
+    return EXIT_SUCCESS;
+}
+
+
+

src/icub/tests/CMakeLists.txt

@@ -27,3 +27,4 @@ add_icub_test(iDynTreeWholeBodyFTsensorEstimate)
 add_icub_test(iDynTreeExportFixedJointsFrames)
 add_icub_test(iDynTreeExternalWrenchesEstimation)
 add_icub_test(iDynTreeiCubExternalWrenchesEstimation)
+add_icub_test(AdjointMatricesConsistency)