Merge pull request #2403 from MegMll/topic/mjcf_warning_and_root_joint

jcarpent · web-flow · commit acf77e86df04 · 2024-09-04T15:32:37.000+02:00
Parser/mjcf : Add warning and fix root joint of mjcf models
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -8,6 +8,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ### Fixed
 - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400))
+- Fix mjcf parser appending of inertias at root joint ([#2403](https://github.com/stack-of-tasks/pinocchio/pull/2403))
 
 ## [3.2.0] - 2024-08-27
 
diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp
@@ -500,15 +500,29 @@ namespace pinocchio
         namespace fs = boost::filesystem;
         MjcfTexture text;
         auto file = el.get_optional<std::string>("<xmlattr>.file");
-        if (!file)
-          throw std::invalid_argument("Only textures with files are supported");
-
-        fs::path filePath(*file);
-        std::string name = getName(el, filePath);
+        auto name_ = el.get_optional<std::string>("<xmlattr>.name");
+        auto type = el.get_optional<std::string>("<xmlattr>.type");
 
-        text.filePath =
-          updatePath(compilerInfo.strippath, compilerInfo.texturedir, modelPath, filePath).string();
+        std::string name;
+        if (name_)
+          name = *name_;
+        else if (type && *type == "skybox")
+          name = *type;
+        if (!file)
+        {
+          std::cout << "Warning - Only texture with files are supported" << std::endl;
+          if (name.empty())
+            throw std::invalid_argument("Textures need a name.");
+        }
+        else
+        {
+          fs::path filePath(*file);
+          name = getName(el, filePath);
 
+          text.filePath =
+            updatePath(compilerInfo.strippath, compilerInfo.texturedir, modelPath, filePath)
+              .string();
+        }
         auto str_v = el.get_optional<std::string>("<xmlattr>.type");
         if (str_v)
           text.textType = *str_v;
@@ -785,15 +799,12 @@ namespace pinocchio
       {
 
         FrameIndex parentFrameId = 0;
-        Inertia inert = Inertia::Zero();
         if (!currentBody.bodyParent.empty())
-        {
           parentFrameId = urdfVisitor.getBodyId(currentBody.bodyParent);
-          inert = currentBody.bodyInertia;
-        }
+
         // get body pose in body parent
         const SE3 bodyPose = currentBody.bodyPlacement;
-
+        Inertia inert = currentBody.bodyInertia;
         SE3 jointInParent = bodyPose * joint.jointPlacement;
         bodyInJoint = joint.jointPlacement.inverse();
         UrdfVisitor::JointType jType;
@@ -1036,7 +1047,8 @@ namespace pinocchio
         // get name and inertia of first root link
         std::string rootLinkName = bodiesList.at(0);
         MjcfBody rootBody = mapOfBodies.find(rootLinkName)->second;
-        urdfVisitor.addRootJoint(rootBody.bodyInertia, rootLinkName);
+        if (rootBody.jointChildren.size() == 0)
+          urdfVisitor.addRootJoint(rootBody.bodyInertia, rootLinkName);
 
         fillReferenceConfig(rootBody);
         for (const auto & entry : bodiesList)
diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp
@@ -940,6 +940,39 @@ BOOST_AUTO_TEST_CASE(adding_keyframes)
   BOOST_CHECK(vect_model == vect_ref);
 }
 
+// Test on which joints inertias are append
+BOOST_AUTO_TEST_CASE(joint_and_inertias)
+{
+  typedef pinocchio::SE3::Vector3 Vector3;
+  typedef pinocchio::SE3::Matrix3 Matrix3;
+
+  std::istringstream xmlData(R"(
+            <mujoco model="testKeyFrame">
+                <worldbody>
+                    <body name="body1">
+                        <freejoint/>
+                        <inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974"
+                        diaginertia="0.00315 0.00388 0.004285"/>
+                    </body>
+                </worldbody>
+                </mujoco>)");
+
+  auto namefile = createTempFile(xmlData);
+
+  pinocchio::Model model_m;
+  pinocchio::mjcf::buildModel(namefile.name(), model_m);
+
+  BOOST_CHECK(model_m.inertias[0].isApprox(pinocchio::Inertia::Zero()));
+
+  Matrix3 inertia_matrix = Eigen::Matrix3d::Zero();
+  inertia_matrix(0, 0) = 0.00315;
+  inertia_matrix(1, 1) = 0.00388;
+  inertia_matrix(2, 2) = 0.004285;
+  pinocchio::Inertia real_inertia(0.629769, Vector3(-0.041018, -0.00014, 0.049974), inertia_matrix);
+
+  BOOST_CHECK(model_m.inertias[1].isApprox(real_inertia));
+}
+
 // Test reference positions and how it's included in keyframe
 BOOST_AUTO_TEST_CASE(reference_positions)
 {
