Account for part/link rotation when saving inertia matrix

CMakeLists.txt

@@ -11,7 +11,6 @@ project(creo2urdf LANGUAGES C CXX
 include(GNUInstallDirs)
 include(FeatureSummary)
 
-# Needed for https://github.com/robotology/idyntree/issues/1065
 find_package(Eigen3 REQUIRED)
 find_package(iDynTree REQUIRED)
 find_package(yaml-cpp REQUIRED)

src/creo2urdf/CMakeLists.txt

@@ -71,6 +71,7 @@ target_link_libraries(creo2urdf PRIVATE iDynTree::idyntree-modelio
                                         iDynTree::idyntree-high-level
                                         yaml-cpp
                                         LibXml2::LibXml2
+                                        Eigen3::Eigen
                                         protk_dllmd_NU
                                         otk_cpp_md
                                         otk_no222_md

src/creo2urdf/src/Creo2Urdf.cpp

@@ -10,7 +10,11 @@
 #include <creo2urdf/Creo2Urdf.h>
 #include <creo2urdf/Utils.h>
 #include <pfcExceptions.h>
+
 #include <iDynTree/PrismaticJoint.h>
+#include <iDynTree/EigenHelpers.h>
+
+#include <Eigen/Core>
 
 void Creo2Urdf::OnCommand() {
 
@@ -120,6 +124,7 @@ void Creo2Urdf::OnCommand() {
         auto link_name = string(component_handle->GetFullName());
 
         iDynTree::Transform root_H_link = iDynTree::Transform::Identity();
+        iDynTree::Transform csys_H_link = iDynTree::Transform::Identity();
 
         std::string urdf_link_name = getRenameElementFromConfig(link_name);
 
@@ -145,9 +150,15 @@ void Creo2Urdf::OnCommand() {
         }
 
         auto mass_prop = pfcSolid::cast(component_handle)->GetMassProperty();
+
+        std::tie(ret, csys_H_link) = getTransformFromPart(component_handle, link_frame_name, scale);
+        if (!ret && warningsAreFatal)
+        {
+            return;
+        }
 
         iDynTree::Link link;
-        link.setInertia(computeSpatialInertiafromCreo(mass_prop, root_H_link, urdf_link_name));
+        link.setInertia(computeSpatialInertiafromCreo(mass_prop, csys_H_link, urdf_link_name));
 
         if (!link.getInertia().isPhysicallyConsistent())
         {
@@ -375,31 +386,55 @@ bool Creo2Urdf::exportModelToUrdf(iDynTree::Model mdl, iDynTree::ModelExporterOp
 iDynTree::SpatialInertia Creo2Urdf::computeSpatialInertiafromCreo(pfcMassProperty_ptr mass_prop, iDynTree::Transform H, const std::string& link_name) {
     auto com = mass_prop->GetGravityCenter();
     auto inertia_tensor = mass_prop->GetCenterGravityInertiaTensor();
-    iDynTree::RotationalInertiaRaw idyn_inertia_tensor = iDynTree::RotationalInertiaRaw::Zero();
+
+    iDynTree::RotationalInertiaRaw idyn_inertia_tensor_csys_orientation = iDynTree::RotationalInertiaRaw::Zero();
+    iDynTree::RotationalInertiaRaw idyn_inertia_tensor_link_orientation = iDynTree::RotationalInertiaRaw::Zero();
+
     bool assigned_inertia_flag = assigned_inertias_map.find(link_name) != assigned_inertias_map.end();
-    for (int i_row = 0; i_row < idyn_inertia_tensor.rows(); i_row++) {
-        for (int j_col = 0; j_col < idyn_inertia_tensor.cols(); j_col++) {
-            if ((assigned_inertia_flag) && (i_row == j_col))
-            {
-                idyn_inertia_tensor.setVal(i_row, j_col, assigned_inertias_map.at(link_name)[i_row]);
+    for (int i_row = 0; i_row < idyn_inertia_tensor_csys_orientation.rows(); i_row++) {
+        for (int j_col = 0; j_col < idyn_inertia_tensor_csys_orientation.cols(); j_col++) {
+            if ((assigned_inertia_flag) && (i_row == j_col)) {
+                // The assigned inertia is already expressed in the link frame
+                idyn_inertia_tensor_link_orientation.setVal(i_row, j_col, assigned_inertias_map.at(link_name)[i_row]);
             }
             else {
-                idyn_inertia_tensor.setVal(i_row, j_col, inertia_tensor->get(i_row, j_col) * scale[i_row] * scale[j_col]);
+                idyn_inertia_tensor_csys_orientation.setVal(i_row, j_col, inertia_tensor->get(i_row, j_col) * scale[i_row] * scale[j_col]);
             }
         }
     }
 
     iDynTree::Position com_child({ com->get(0) * scale[0] , com->get(1) * scale[1], com->get(2) * scale[2] });
-    com_child = H.inverse() * com_child;  // TODO verify
+
+    // Account for root_H_link transformation
+    // See https://github.com/icub-tech-iit/ergocub-software/issues/224#issuecomment-1985692598 for full contents
+
+    // The COM returned by Creo's GetGravityCenter seems to be expressed in the root frame, so we need 
+    // to transform it back to the link frame before passing it to iDynTree's fromRotationalInertiaWrtCenterOfMass
+    com_child = H.inverse() * com_child;
+
+    // The inertia returned by Creo's GetCenterGravityInertiaTensor seems to be expressed with the COM as the
+    // point in which it is expressed, and with the orientation of the root link, so we rotate it back with
+    // the orientation of the link frame, unless an assignedInertia is used
+    if (!assigned_inertia_flag) {
+        // Note, this auto-defined methods are Eigen::Map, so they are reference to data that remains
+        // stored in the original iDynTree object, see https://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
+        auto inertia_tensor_root = iDynTree::toEigen(idyn_inertia_tensor_csys_orientation);
+        auto inertia_tensor_link = iDynTree::toEigen(idyn_inertia_tensor_link_orientation);
+        auto csys_R_link = iDynTree::toEigen(H.getRotation());
+
+        // See Equation 15 of https://ocw.mit.edu/courses/16-07-dynamics-fall-2009/dd277ec654440f4c2b5b07d6c286c3fd_MIT16_07F09_Lec26.pdf
+        inertia_tensor_link = csys_R_link.transpose()*inertia_tensor_root*csys_R_link;
+    }
+
     double mass{ 0.0 };
     if (config["assignedMasses"][link_name].IsDefined()) {
         mass = config["assignedMasses"][link_name].as<double>();
     }
     else {
         mass = mass_prop->GetMass();
     }
-    iDynTree::SpatialInertia sp_inertia(mass, com_child, idyn_inertia_tensor);
-    sp_inertia.fromRotationalInertiaWrtCenterOfMass(mass, com_child, idyn_inertia_tensor);
+    iDynTree::SpatialInertia sp_inertia(mass, com_child, idyn_inertia_tensor_link_orientation);
+    sp_inertia.fromRotationalInertiaWrtCenterOfMass(mass, com_child, idyn_inertia_tensor_link_orientation);
 
     return sp_inertia;
 }