From 894ed72bf3b662089e0c194b2f1db3676a44210c Mon Sep 17 00:00:00 2001
From: Silvio <silvio.traversaro@iit.it>
Date: Mon, 18 Jun 2018 13:26:50 +0200
Subject: [PATCH] Add test to check that the FT sensors are correctly oriented

See https://github.com/robotology/icub-model-generator/issues/92 for more info
---
 tests/icub-model-test.cpp | 55 ++++++++++++++++++++++++++++++++++++++-
 1 file changed, 54 insertions(+), 1 deletion(-)

diff --git a/tests/icub-model-test.cpp b/tests/icub-model-test.cpp
index ffe82443..5acb6fc1 100644
--- a/tests/icub-model-test.cpp
+++ b/tests/icub-model-test.cpp
@@ -280,6 +280,54 @@ bool checkFTSensorsAreEvenAndNotNull(iDynTree::ModelLoader & mdlLoader)
     return true;
 }
 
+bool checkFTSensorIsCorrectlyOriented(iDynTree::KinDynComputations & comp,
+                                      const iDynTree::Rotation& expected,
+                                      const std::string& sensorName)
+{
+    // Depending on the icub model, the sensor could be absent
+    if (!comp.model().isFrameNameUsed(sensorName))
+    {
+        return true;
+    }
+
+    iDynTree::Rotation actual = comp.getRelativeTransform("root_link", sensorName).getRotation();
+
+    if (!checkMatrixAreEqual(expected, actual, 1e-3))
+    {
+        std::cerr << "icub-model-test : transform between root_link and " << sensorName << " is not the expected one, test failed." << std::endl;
+        std::cerr << "icub-model-test : Expected transform : " << expected.toString() << std::endl;
+        std::cerr << "icub-model-test : Actual transform : " << actual.toString() << std::endl;
+        return false;
+    }
+
+    return true;
+}
+
+
+/**
+ * Check that the F/T sensor have the correct orientation.
+ *
+ * See https://github.com/robotology/icub-model-generator/issues/92
+ */
+bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp)
+{
+    // The rotation of all the F/T sensors in the iCub robot when in zero position are the same
+    iDynTree::Rotation rootLink_R_sensorFrameExpected =
+        iDynTree::Rotation(-1.0,  0.0,  0.0,
+                            0.0,  1.0,  0.0,
+                            0.0,  0.0, -1.0);
+
+
+    bool ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "l_arm_ft_sensor");
+    ok = ok && checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "r_arm_ft_sensor");
+    ok = ok && checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "l_leg_ft_sensor");
+    ok = ok && checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "r_leg_ft_sensor");
+    ok = ok && checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "l_foot_ft_sensor");
+    ok = ok && checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "r_foot_ft_sensor");
+
+    return ok;
+}
+
 int main(int argc, char ** argv)
 {
     yarp::os::Property prop;
@@ -343,12 +391,17 @@ int main(int argc, char ** argv)
     iDynTree::ModelLoader mdlLoader;
     mdlLoader.loadModelFromFile(modelPath);
 
-
     if( !checkFTSensorsAreEvenAndNotNull(mdlLoader) )
     {
         return EXIT_FAILURE;
     }
 
+    if (!checkFTSensorsAreCorrectlyOriented(comp))
+    {
+        return EXIT_FAILURE;
+    }
+
+
     std::cerr << "Check for model " << modelPath << " concluded correctly!" << std::endl;
 
     return EXIT_SUCCESS;