stack-of-tasks · jorisv · Nov 13, 2024 · Nov 13, 2024 · Nov 13, 2024 · Nov 13, 2024
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -9,6 +9,10 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 ### Added
 - Add `pinocchio_python_parser` target ([#2475](https://github.com/stack-of-tasks/pinocchio/pull/2475))
 
+### Fixed
+- Fix mjcf parsing of armature and of the default tag in models ([#2477](https://github.com/stack-of-tasks/pinocchio/pull/2477))
+- Fix undefined behavior when using the site attribute in mjcf ([#2477](https://github.com/stack-of-tasks/pinocchio/pull/2477))
+
 ### Changed
 - On GNU/Linux and macOS, hide all symbols by default ([#2469](https://github.com/stack-of-tasks/pinocchio/pull/2469))
 

diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp
@@ -135,7 +135,7 @@ namespace pinocchio
         Eigen::VectorXd damping;
 
         // Armature inertia created by this joint
-        double armature = 0.;
+        Eigen::VectorXd armature;
         // Dry friction.
         double frictionLoss = 0.;
 
@@ -149,9 +149,9 @@ namespace pinocchio
           maxConfig = Eigen::VectorXd::Constant(1, infty);
           springStiffness = Eigen::VectorXd::Constant(1, v);
           springReference = Eigen::VectorXd::Constant(1, v);
-          ;
           friction = Eigen::VectorXd::Constant(1, 0.);
           damping = Eigen::VectorXd::Constant(1, 0.);
+          armature = Eigen::VectorXd::Constant(1, 0.);
         }
 
         /// @brief Set dimension to the limits to match the joint nq and nv.
@@ -446,7 +446,7 @@ namespace pinocchio
         /// @brief Go through the default part of the file and get all the class name. Fill the
         /// mapOfDefault for later use.
         /// @param el ptree element. Root of the default
-        void parseDefault(ptree & el, const ptree & parent);
+        void parseDefault(ptree & el, const ptree & parent, const std::string & parentTag);
 
         /// @brief Go through the main body of the mjcf file "worldbody" to get all the info ready
         /// to create the model.

diff --git a/src/parsers/mjcf/mjcf-graph-geom.cpp b/src/parsers/mjcf/mjcf-graph-geom.cpp
@@ -410,7 +410,13 @@ namespace pinocchio
           geomName =
             currentBody.bodyName + "Geom_" + std::to_string(currentBody.geomChildren.size());
 
-        // ChildClass < Class < Real Joint
+        // default < ChildClass < Class < Real Joint
+        if (currentGraph.mapOfClasses.find("mujoco_default") != currentGraph.mapOfClasses.end())
+        {
+          const MjcfClass & classD = currentGraph.mapOfClasses.at("mujoco_default");
+          if (auto geom_p = classD.classElement.get_child_optional("geom"))
+            goThroughElement(*geom_p, currentGraph);
+        }
         //  childClass
         if (currentBody.childClass != "")
         {
@@ -474,7 +480,13 @@ namespace pinocchio
           siteName =
             currentBody.bodyName + "Site_" + std::to_string(currentBody.siteChildren.size());
 
-        // ChildClass < Class < Real Joint
+        // default < ChildClass < Class < Real Joint
+        if (currentGraph.mapOfClasses.find("mujoco_default") != currentGraph.mapOfClasses.end())
+        {
+          const MjcfClass & classD = currentGraph.mapOfClasses.at("mujoco_default");
+          if (auto site_p = classD.classElement.get_child_optional("site"))
+            goThroughElement(*site_p, currentGraph);
+        }
         //  childClass
         if (currentBody.childClass != "")
         {

diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp
@@ -127,7 +127,7 @@ namespace pinocchio
         ret.minConfig = Vector::Constant(Nq, -1.01);
         ret.friction = Vector::Constant(Nv, 0.);
         ret.damping = Vector::Constant(Nv, 0.);
-        ret.armature = armature;
+        ret.armature = Vector::Constant(Nv, armature[0]);
         ret.frictionLoss = frictionLoss;
         ret.springStiffness = springStiffness;
         ret.springReference = springReference;
@@ -160,6 +160,10 @@ namespace pinocchio
         ret.springReference.tail(1) = range.springReference;
         ret.springStiffness.conservativeResize(springStiffness.size() + 1);
         ret.springStiffness.tail(1) = range.springStiffness;
+
+        ret.armature.conservativeResize(armature.size() + Nv);
+        ret.armature.tail(Nv) = range.armature;
+
         return ret;
       }
 
@@ -208,7 +212,7 @@ namespace pinocchio
 
         value = el.get_optional<double>("<xmlattr>.armature");
         if (value)
-          range.armature = *value;
+          range.armature[0] = *value;
 
         // friction loss
         value = el.get_optional<double>("<xmlattr>.frictionloss");
@@ -251,7 +255,13 @@ namespace pinocchio
         // Placement
         jointPlacement = currentGraph.convertPosition(el);
 
-        // ChildClass < Class < Real Joint
+        // default < ChildClass < Class < Real Joint
+        if (currentGraph.mapOfClasses.find("mujoco_default") != currentGraph.mapOfClasses.end())
+        {
+          const MjcfClass & classD = currentGraph.mapOfClasses.at("mujoco_default");
+          if (auto joint_p = classD.classElement.get_child_optional("joint"))
+            goThroughElement(*joint_p, use_limit, currentGraph.compilerInfo);
+        }
         //  childClass
         if (currentBody.childClass != "")
         {
@@ -545,7 +555,13 @@ namespace pinocchio
         else
           throw std::invalid_argument("Material was given without a name");
 
-        // Class < Attributes
+        // default < Class < Attributes
+        if (mapOfClasses.find("mujoco_default") != mapOfClasses.end())
+        {
+          const MjcfClass & classD = mapOfClasses.at("mujoco_default");
+          if (auto mat_p = classD.classElement.get_child_optional("material"))
+            mat.goThroughElement(*mat_p);
+        }
         auto cl_s = el.get_optional<std::string>("<xmlattr>.class");
         if (cl_s)
         {
@@ -600,9 +616,10 @@ namespace pinocchio
         }
       }
 
-      void MjcfGraph::parseDefault(ptree & el, const ptree & parent)
+      void MjcfGraph::parseDefault(ptree & el, const ptree & parent, const std::string & parentTag)
       {
         boost::optional<std::string> nameClass;
+        // Classes
         for (ptree::value_type & v : el)
         {
           if (v.first == "<xmlattr>")
@@ -619,8 +636,15 @@ namespace pinocchio
             else
               throw std::invalid_argument("Class does not have a name. Cannot parse model.");
           }
-          if (v.first == "default")
-            parseDefault(v.second, el);
+          else if (v.first == "default")
+            parseDefault(v.second, el, v.first);
+          else if (parentTag == "mujoco" && v.first != "<xmlattr>")
+          {
+            MjcfClass classDefault;
+            classDefault.className = "mujoco_default";
+            classDefault.classElement = el;
+            mapOfClasses.insert(std::make_pair("mujoco_default", classDefault));
+          }
         }
       }
 
@@ -808,7 +832,7 @@ namespace pinocchio
             parseCompiler(el.get_child("compiler"));
 
           if (v.first == "default")
-            parseDefault(el.get_child("default"), el);
+            parseDefault(el.get_child("default"), el, "mujoco");
 
           if (v.first == "asset")
             parseAsset(el.get_child("asset"));
@@ -898,7 +922,9 @@ namespace pinocchio
 
         // Add armature info
         JointIndex j_id = urdfVisitor.getJointId(joint.jointName);
-        urdfVisitor.model.armature[static_cast<Eigen::Index>(j_id) - 1] = range.armature;
+        urdfVisitor.model.armature.segment(
+          urdfVisitor.model.joints[j_id].idx_v(), urdfVisitor.model.joints[j_id].nv()) =
+          range.armature;
       }
 
       void MjcfGraph::fillModel(const std::string & nameOfBody)
@@ -912,7 +938,7 @@ namespace pinocchio
         if (!currentBody.bodyParent.empty())
           parentFrameId = urdfVisitor.getBodyId(currentBody.bodyParent);
 
-        const Frame & frame = urdfVisitor.model.frames[parentFrameId];
+        Frame frame = urdfVisitor.model.frames[parentFrameId];
         // get body pose in body parent
         const SE3 bodyPose = currentBody.bodyPlacement;
         Inertia inert = currentBody.bodyInertia;
@@ -1007,7 +1033,9 @@ namespace pinocchio
           FrameIndex jointFrameId = urdfVisitor.model.addJointFrame(joint_id, (int)parentFrameId);
           urdfVisitor.appendBodyToJoint(jointFrameId, inert, bodyInJoint, nameOfBody);
 
-          urdfVisitor.model.armature[static_cast<Eigen::Index>(joint_id) - 1] = rangeCompo.armature;
+          urdfVisitor.model.armature.segment(
+            urdfVisitor.model.joints[joint_id].idx_v(), urdfVisitor.model.joints[joint_id].nv()) =
+            rangeCompo.armature;
         }
 
         FrameIndex previousFrameId = urdfVisitor.model.frames.size();

diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp
@@ -386,7 +386,7 @@ BOOST_AUTO_TEST_CASE(merge_default)
   MjcfGraph::UrdfVisitor visitor(model);
 
   MjcfGraph graph(visitor, "fakeMjcf");
-  graph.parseDefault(ptr.get_child("default"), ptr);
+  graph.parseDefault(ptr.get_child("default"), ptr, "default");
 
   std::unordered_map<std::string, pt::ptree> TrueMap;
 
@@ -974,6 +974,47 @@ BOOST_AUTO_TEST_CASE(joint_and_inertias)
   BOOST_CHECK(model_m.inertias[1].isApprox(real_inertia));
 }
 
+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>
+                  <joint armature="1" damping="1" limited="true"/>
+                </default>
+                <worldbody>
+                    <body name="link0">
+                        <body name="link1" pos="0 0 0">
+                            <joint name="joint1" type="hinge" axis="1 0 0" armature="1.3"/>
+                            <joint name="joint2" type="hinge" axis="0 1 0" armature="2.4"/>
+                            <joint name="joint3" type="hinge" axis="0 0 1" armature="0.4"/>
+                            <body pos=".2 0 0" name="body2">
+                              <joint type="ball"/>
+                        </body>
+                        </body>
+                    </body>
+                </worldbody>
+            </mujoco>)");
+
+  auto namefile = createTempFile(xmlData);
+
+  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
+  pinocchio::Model model_m;
+  MjcfGraph::UrdfVisitor visitor(model_m);
+
+  MjcfGraph graph(visitor, "fakeMjcf");
+  graph.parseGraphFromXML(namefile.name());
+  graph.parseRootTree();
+
+  Eigen::VectorXd armature_real(model_m.nv);
+  armature_real << 1.3, 2.4, 0.4, 1, 1, 1;
+
+  for (size_t i = 0; i < size_t(model_m.nv); i++)
+    BOOST_CHECK_EQUAL(model_m.armature[i], armature_real[i]);
+}
+
 // Test reference positions and how it's included in keyframe
 BOOST_AUTO_TEST_CASE(reference_positions)
 {