Update the model of iCubGenova02 with the "plus" modifications (#106).

* Add model for iCubGenova02 plus. * Update the model of iCubGenova02 with the "plus" modifications and delete iCubGenova02_plus. * Update test for the 2.5+ models. * Correct typo.
robotology · Sep 7, 2018 · bada6e5 · bada6e5
1 parent 7cc507e
commit bada6e5
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 6 deletions.
diff --git a/README.md b/README.md
@@ -38,7 +38,7 @@ Both generation pipelines are still in `a work in progress` state, and several i
 | `iCubGazeboV2_5`     | simmechanics | v2.5 with backpack, joint damping, and inertias of some links modified to run smoothly in Gazebo |
 | `iCubGazeboV2_5_plus`| simmechanics | v2.5+ with backpack, joint damping, and inertias of some links modified to run smoothly in Gazebo |
 | `iCubGenova01`       | simmechanics | v2.5 without backpack           |
-| `iCubGenova02`       | simmechanics | v2.5   with backpack            |
+| `iCubGenova02`       | simmechanics | v2.5+   with backpack           |
 | `iCubGenova03`       | dh           | v2 with legs v1 and feet v2.5   |
 | `iCubGenova04`       | simmechanics | v2.5   with backpack            |
 | `iCubGenova04_plus`  | simmechanics | v2.5+   with backpack           |

diff --git a/simmechanics/CMakeLists.txt b/simmechanics/CMakeLists.txt
@@ -252,9 +252,10 @@ generate_icub_simmechanics(YARP_ROBOT_NAME iCubGenova04_plus
                           ICUB_PLUS)
 
 generate_icub_simmechanics(YARP_ROBOT_NAME iCubGenova02
-                           SIMMECHANICS_XML "icub2_5/ICUB_2-5_BB_SIM_MODEL.xml"
+                           SIMMECHANICS_XML "icub2_5/ICUB_2-5_plus_BB_SIM_MODEL.xml"
                            YAML_TEMPLATE "icub2_5/ICUB_2-5_BB_simmechanics_options.yaml.in"
-                           CSV_TEMPLATE "icub2_5/ICUB_2-5_BB_joint_parameters.csv.in")
+                           CSV_TEMPLATE "icub2_5/ICUB_2-5_BB_joint_parameters.csv.in"
+                           ICUB_PLUS)
 
 generate_icub_simmechanics(YARP_ROBOT_NAME iCubGenova04
                            SIMMECHANICS_XML "icub2_5/ICUB_2-5_BB_SIM_MODEL.xml"

diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
@@ -24,9 +24,9 @@ endif()
 # Model generated with simmechanics
 if( ICUB_MODEL_GENERATE_SIMMECHANICS )
     add_icub_model_test(iCubGazeboV2_5)
+    add_icub_model_test(iCubGazeboV2_5_plus)
     add_icub_model_test(iCubGenova01)
     add_icub_model_test(iCubGenova02)
     add_icub_model_test(iCubGenova04)
     add_icub_model_test(iCubDarmstadt01)
 endif()
-
diff --git a/tests/icub-model-test.cpp b/tests/icub-model-test.cpp
@@ -316,14 +316,21 @@ bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp)
         iDynTree::Rotation(-1.0,  0.0,  0.0,
                             0.0,  1.0,  0.0,
                             0.0,  0.0, -1.0);
+    // The rotation of the foot F/T sensors in the iCub 2.5+ is different
+    iDynTree::Rotation rootLink_R_foot_sensorFrameExpected_plus =
+        iDynTree::Rotation(-0.965926, 0.0,  0.258819,
+                            0.0,      1.0,  0.0,
+                           -0.258819, 0.0, -0.965926);
 
+    bool isPlusModel = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_foot_sensorFrameExpected_plus, "l_foot_ft_sensor")
+                       && checkFTSensorIsCorrectlyOriented(comp, rootLink_R_foot_sensorFrameExpected_plus, "r_foot_ft_sensor");
 
     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");
+    ok = ok && (checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "l_foot_ft_sensor") || isPlusModel);
+    ok = ok && (checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpected, "r_foot_ft_sensor") || isPlusModel);
 
     return ok;
 }