Merge pull request #2526 from JafarAbdi/fix_default_eulerseq

Mjcf: Use xyz as a default value for eulerseq compiler option
stack-of-tasks · Dec 22, 2024 · c0d86b5 · c0d86b5
2 parents 0d820be + affffd3
commit c0d86b5
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 1 deletion.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -6,6 +6,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+### 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))
+
 ## [3.3.1] - 2024-12-13
 
 ### Added

diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp
@@ -52,7 +52,7 @@ namespace pinocchio
         // Value for angle conversion (Mujoco default - degrees)
         double angle_converter = boost::math::constants::pi<double>() / 180.0;
         // Euler Axis to use to convert angles representation to quaternion
-        Eigen::Matrix3d mapEulerAngles;
+        Eigen::Matrix3d mapEulerAngles = Eigen::Matrix3d::Identity();
 
         // Value to crop the mass (if mass < boundMass, mass = boundMass)
         double boundMass = 0;

diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp
@@ -1328,4 +1328,33 @@ BOOST_AUTO_TEST_CASE(test_contact_parsing)
   }
 }
 
+BOOST_AUTO_TEST_CASE(test_default_eulerseq)
+{
+  std::istringstream xmlData(R"(<mujoco model="arm">
+                                  <compiler angle="radian" />
+                                  <worldbody>
+                                    <body name="base">
+                                      <body name="body" pos="0 0 0.01" euler="1.57 0 0">
+                                      </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();
+
+  pinocchio::SE3 placement(
+    Eigen::AngleAxisd(1.57, Eigen::Vector3d::UnitX()).toRotationMatrix(),
+    Eigen::Vector3d(0.0, 0.0, 0.01));
+
+  BOOST_CHECK(graph.mapOfBodies["body"].bodyPlacement.isApprox(placement));
+}
+
 BOOST_AUTO_TEST_SUITE_END()