Merge pull request #2548 from MegMll/topic/fix_mjcf_site

jorisv · web-flow · commit 6acfc6be98d9 · 2025-01-10T16:55:03.000+01:00
Mjcf Parser : Fix adding the sites frames
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -6,14 +6,17 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+### Added
+- Add parsing meshes with vertices for MJCF format ([#2537](https://github.com/stack-of-tasks/pinocchio/pull/2537))
+
 ### Fixed
 - Fix mjcf Euler angle parsing: use xyz as a default value for eulerseq compiler option ([#2526](https://github.com/stack-of-tasks/pinocchio/pull/2526))
 - Fix variable naming in Python ([#2530](https://github.com/stack-of-tasks/pinocchio/pull/2530))
 - Fix aba explicit template instantiation ([#2541](https://github.com/stack-of-tasks/pinocchio/pull/2541))
-- Add parsing meshes with vertices for MJCF format ([#2537](https://github.com/stack-of-tasks/pinocchio/pull/2537))
 - CMake: fix RPATH on macos ([#2546](https://github.com/stack-of-tasks/pinocchio/pull/2546))
 - Fix aba explicit template instantiation ([#2541](https://github.com/stack-of-tasks/pinocchio/pull/2541))
 - Fix mjcf parsing of keyframe qpos with newlines ([#2535](https://github.com/stack-of-tasks/pinocchio/pull/2535))
+- Fix sites parsing for MJCF format ([#2548](https://github.com/stack-of-tasks/pinocchio/pull/2548))
 
 
 ## [3.3.1] - 2024-12-13
diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp
@@ -59,13 +59,15 @@ namespace pinocchio
         else
         {
           if (filePath.extension().empty())
-            throw std::invalid_argument("Cannot find extension for one of the mesh/texture");
+            PINOCCHIO_THROW_PRETTY(
+              std::invalid_argument, "Cannot find extension for one of the mesh/texture");
 
           auto st = filePath.stem();
           if (!st.empty())
             return st.string();
           else
-            throw std::invalid_argument("Cannot find a name for one of the mesh.texture");
+            PINOCCHIO_THROW_PRETTY(
+              std::invalid_argument, "Cannot find a name for one of the mesh.texture");
         }
       }
 
@@ -172,7 +174,8 @@ namespace pinocchio
       {
 
         if (!use_limits && el.get_optional<std::string>("<xmlattr>.range"))
-          throw std::invalid_argument("Range limit is specified but it was not specify to use it.");
+          PINOCCHIO_THROW_PRETTY(
+            std::invalid_argument, "Range limit is specified but it was not specify to use it.");
 
         // Type
         auto type_s = el.get_optional<std::string>("<xmlattr>.type");
@@ -227,7 +230,8 @@ namespace pinocchio
           else if (jointType == "hinge")
             posRef = currentCompiler.convertAngle(*value);
           else
-            throw std::invalid_argument(
+            PINOCCHIO_THROW_PRETTY(
+              std::invalid_argument,
               "Reference position can only be used with hinge or slide joints.");
         }
       }
@@ -358,7 +362,8 @@ namespace pinocchio
         double mass = std::max(el.get<double>("<xmlattr>.mass"), compilerInfo.boundMass);
         ;
         if (mass < 0)
-          throw std::invalid_argument("Mass of body is not supposed to be negative");
+          PINOCCHIO_THROW_PRETTY(
+            std::invalid_argument, "Mass of body is not supposed to be negative");
 
         Inertia::Vector3 com;
         auto com_s = el.get_optional<std::string>("<xmlattr>.pos");
@@ -410,7 +415,8 @@ namespace pinocchio
         auto chcl_s = childClass;
         // if inertiafromgeom is false and inertia does not exist - throw
         if (!compilerInfo.inertiafromgeom && !el.get_child_optional("inertial"))
-          throw std::invalid_argument("Cannot get inertia from geom and no inertia was found");
+          PINOCCHIO_THROW_PRETTY(
+            std::invalid_argument, "Cannot get inertia from geom and no inertia was found");
 
         bool usegeominertia = false;
         if (compilerInfo.inertiafromgeom)
@@ -524,7 +530,7 @@ namespace pinocchio
         {
           std::cout << "Warning - Only texture with files are supported" << std::endl;
           if (name.empty())
-            throw std::invalid_argument("Textures need a name.");
+            PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Textures need a name.");
         }
         else
         {
@@ -553,7 +559,7 @@ namespace pinocchio
         if (n)
           name = *n;
         else
-          throw std::invalid_argument("Material was given without a name");
+          PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Material was given without a name");
 
         // default < Class < Attributes
         if (mapOfClasses.find("mujoco_default") != mapOfClasses.end())
@@ -644,7 +650,7 @@ namespace pinocchio
             parseTexture(v.second);
 
           if (v.first == "hfield")
-            throw std::invalid_argument("hfields are not supported yet");
+            PINOCCHIO_THROW_PRETTY(std::invalid_argument, "hfields are not supported yet");
         }
       }
 
@@ -666,7 +672,8 @@ namespace pinocchio
               mapOfClasses.insert(std::make_pair(*nameClass, classDefault));
             }
             else
-              throw std::invalid_argument("Class does not have a name. Cannot parse model.");
+              PINOCCHIO_THROW_PRETTY(
+                std::invalid_argument, "Class does not have a name. Cannot parse model.");
           }
           else if (v.first == "default")
             parseDefault(v.second, el, v.first);
@@ -735,8 +742,10 @@ namespace pinocchio
         {
           std::string eulerseq = *eulerS;
           if (eulerseq.find_first_not_of("xyzXYZ") != std::string::npos || eulerseq.size() != 3)
-            throw std::invalid_argument(
-              "Model tried to use euler angles but euler sequence is wrong");
+          {
+            PINOCCHIO_THROW_PRETTY(
+              std::invalid_argument, "Model tried to use euler angles but euler sequence is wrong");
+          }
           else
           {
             // get index combination
@@ -764,7 +773,7 @@ namespace pinocchio
                 compilerInfo.mapEulerAngles.col(ci) = Eigen::Vector3d::UnitZ();
                 break;
               default:
-                throw std::invalid_argument("Euler Axis does not exist");
+                PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Euler Axis does not exist");
                 break;
               }
             }
@@ -817,7 +826,7 @@ namespace pinocchio
           if (body1)
             eq.body1 = *body1;
           else
-            throw std::invalid_argument("Equality constraint needs a first body");
+            PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Equality constraint needs a first body");
 
           // get the name of second body
           auto body2 = v.second.get_optional<std::string>("<xmlattr>.body2");
@@ -846,7 +855,8 @@ namespace pinocchio
         if (pt.get_child_optional("mujoco"))
           el = pt.get_child("mujoco");
         else
-          throw std::invalid_argument("This is not a standard mujoco model. Cannot parse it.");
+          PINOCCHIO_THROW_PRETTY(
+            std::invalid_argument, "This is not a standard mujoco model. Cannot parse it.");
 
         for (const ptree::value_type & v : el)
         {
@@ -857,7 +867,8 @@ namespace pinocchio
             if (n_s)
               modelName = *n_s;
             else
-              throw std::invalid_argument("Model is missing a name. Cannot parse it");
+              PINOCCHIO_THROW_PRETTY(
+                std::invalid_argument, "Model is missing a name. Cannot parse it");
           }
 
           if (v.first == "compiler")
@@ -945,7 +956,7 @@ namespace pinocchio
           jType = UrdfVisitor::REVOLUTE;
         }
         else
-          throw std::invalid_argument("Unknown joint type");
+          PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Unknown joint type");
 
         urdfVisitor.addJointAndBody(
           jType, joint.axis, parentFrameId, jointInParent, joint.jointName, inert, bodyInJoint,
@@ -1017,7 +1028,8 @@ namespace pinocchio
           for (const auto & joint : currentBody.jointChildren)
           {
             if (joint.jointType == "free")
-              throw std::invalid_argument("Joint Composite trying to be created with a freeFlyer");
+              PINOCCHIO_THROW_PRETTY(
+                std::invalid_argument, "Joint Composite trying to be created with a freeFlyer");
 
             SE3 jointInParent = bodyPose * joint.jointPlacement;
             bodyInJoint = joint.jointPlacement.inverse();
@@ -1052,7 +1064,8 @@ namespace pinocchio
               rangeCompo = rangeCompo.concatenate<1, 1>(joint.range);
             }
             else
-              throw std::invalid_argument("Unknown joint type trying to be parsed.");
+              PINOCCHIO_THROW_PRETTY(
+                std::invalid_argument, "Unknown joint type trying to be parsed.");
 
             prevJointPlacement = jointInParent;
           }
@@ -1070,12 +1083,13 @@ namespace pinocchio
             rangeCompo.armature;
         }
 
-        FrameIndex previousFrameId = urdfVisitor.model.frames.size();
+        FrameIndex bodyId = urdfVisitor.model.getFrameId(nameOfBody, BODY);
+        frame = urdfVisitor.model.frames[bodyId];
         for (const auto & site : currentBody.siteChildren)
         {
           SE3 placement = bodyInJoint * site.sitePlacement;
-          previousFrameId = urdfVisitor.model.addFrame(
-            Frame(site.siteName, frame.parentJoint, previousFrameId, placement, OP_FRAME));
+          urdfVisitor.model.addFrame(
+            Frame(site.siteName, frame.parentJoint, bodyId, placement, OP_FRAME));
         }
       }
 
@@ -1151,7 +1165,7 @@ namespace pinocchio
           urdfVisitor.model.referenceConfigurations.insert(std::make_pair(keyName, qpos));
         }
         else
-          throw std::invalid_argument("Keyframe size does not match model size");
+          PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Keyframe size does not match model size");
       }
 
       void MjcfGraph::parseContactInformation(
@@ -1188,7 +1202,6 @@ namespace pinocchio
       void MjcfGraph::parseRootTree()
       {
         urdfVisitor.setName(modelName);
-
         // get name and inertia of first root link
         std::string rootLinkName = bodiesList.at(0);
         MjcfBody rootBody = mapOfBodies.find(rootLinkName)->second;
diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp
@@ -980,7 +980,7 @@ BOOST_AUTO_TEST_CASE(armature)
 {
   typedef pinocchio::SE3::Vector3 Vector3;
   typedef pinocchio::SE3::Matrix3 Matrix3;
-  std::cout << " Armature ------------ " << std::endl;
+
   std::istringstream xmlData(R"(
             <mujoco model="model_RX">
                 <default>
@@ -1120,6 +1120,9 @@ BOOST_AUTO_TEST_CASE(adding_site)
   pinocchio::SE3 real_placement(rotation_matrix, Vector3(0.03, 0, -0.05));
 
   BOOST_CHECK(model_m.frames[model_m.getFrameId("testSite")].placement.isApprox(real_placement));
+  BOOST_CHECK(
+    model_m.frames[model_m.getFrameId("testSite")].parentJoint
+    == model_m.frames[model_m.getFrameId("body3")].parentJoint);
 }
 
 /// @brief test that a fixed model is well parsed
