Merge branch 'devel' into topic/fix_aba_template_instantiation

stack-of-tasks · Jan 3, 2025 · a6fb782 · a6fb782
2 parents a5bc9d0 + 3ce5cf1
commit a6fb782
Show file tree

Hide file tree

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

diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp
@@ -209,6 +209,8 @@ namespace pinocchio
         Eigen::Vector3d scale = Eigen::Vector3d::Constant(1);
         // Path to the mesh file
         std::string filePath;
+        // Vertices of the mesh
+        Eigen::MatrixX3d vertices;
       };
 
       /// @brief All informations related to a texture are stored here

diff --git a/src/parsers/mjcf/mjcf-graph-geom.cpp b/src/parsers/mjcf/mjcf-graph-geom.cpp
@@ -75,6 +75,19 @@ namespace pinocchio
         if (geom.geomType == "mesh")
         {
           MjcfMesh currentMesh = currentGraph.mapOfMeshes.at(geom.meshName);
+          if (currentMesh.vertices.size() > 0)
+          {
+            auto vertices = currentMesh.vertices;
+            // Scale vertices
+            for (std::size_t i = 0; i < vertices.rows(); ++i)
+              vertices.row(i) = vertices.row(i).cwiseProduct(currentMesh.scale.transpose());
+            auto model = std::make_shared<hpp::fcl::BVHModel<fcl::OBBRSS>>();
+            model->beginModel();
+            model->addVertices(vertices);
+            model->endModel();
+            model->buildConvexHull(true, "Qt");
+            return model->convex;
+          }
           meshPath = currentMesh.filePath;
           meshScale = currentMesh.scale;
           hpp::fcl::BVHModelPtr_t bvh = meshLoader->load(meshPath, meshScale);

diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp
@@ -582,20 +582,52 @@ namespace pinocchio
 
         MjcfMesh mesh;
         auto file = el.get_optional<std::string>("<xmlattr>.file");
-        if (!file)
-          throw std::invalid_argument("Only meshes with files are supported");
-
-        fs::path filePath(*file);
-        std::string name = getName(el, filePath);
-
-        mesh.filePath =
-          updatePath(compilerInfo.strippath, compilerInfo.meshdir, modelPath, filePath).string();
-
         auto scale = el.get_optional<std::string>("<xmlattr>.scale");
         if (scale)
           mesh.scale = internal::getVectorFromStream<3>(*scale);
+        if (file)
+        {
+          fs::path filePath(*file);
+          std::string name = getName(el, filePath);
+
+          mesh.filePath =
+            updatePath(compilerInfo.strippath, compilerInfo.meshdir, modelPath, filePath).string();
+          mapOfMeshes.insert(std::make_pair(name, mesh));
+          return;
+        }
+
+        // Handle vertex-based mesh
+        auto vertex = el.get_optional<std::string>("<xmlattr>.vertex");
+        if (!vertex)
+        {
+          PINOCCHIO_THROW_PRETTY(
+            std::invalid_argument, "Only meshes with files/vertices are supported.")
+        }
+
+        auto name = el.get_optional<std::string>("<xmlattr>.name");
+        if (!name)
+        {
+          PINOCCHIO_THROW_PRETTY(
+            std::invalid_argument, "Mesh with vertices without a name is not supported");
+        }
 
-        mapOfMeshes.insert(std::make_pair(name, mesh));
+        // Parse and validate vertices
+        Eigen::VectorXd meshVertices = internal::getUnknownSizeVectorFromStream(*vertex);
+        if (meshVertices.size() % 3 != 0)
+        {
+          PINOCCHIO_THROW_PRETTY(
+            std::invalid_argument, "Number of vertices is not a multiple of 3");
+        }
+
+        // Convert to 3D vertex matrix
+        const auto numVertices = meshVertices.size() / 3;
+        Eigen::MatrixX3d vertices(numVertices, 3);
+        for (auto i = 0; i < numVertices; ++i)
+        {
+          vertices.row(i) = meshVertices.segment<3>(3 * i).transpose();
+        }
+        mesh.vertices = vertices;
+        mapOfMeshes.insert(std::make_pair(*name, mesh));
       }
 
       void MjcfGraph::parseAsset(const ptree & el)

diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp
@@ -1357,4 +1357,45 @@ BOOST_AUTO_TEST_CASE(test_default_eulerseq)
   BOOST_CHECK(graph.mapOfBodies["body"].bodyPlacement.isApprox(placement));
 }
 
+/// @brief Test parsing a mesh with vertices
+/// @param
+BOOST_AUTO_TEST_CASE(parse_mesh_with_vertices)
+{
+  std::istringstream xmlDataNoStrip(R"(<mujoco model="parseVertices">
+                                    <asset>
+                                      <mesh name="chasis" scale=".01 .006 .0015"
+                                        vertex=" 9   2   0
+                                                -10  10  10
+                                                 9  -2   0
+                                                 10  3  -10
+                                                 10 -3  -10
+                                                -8   10 -10
+                                                -10 -10  10
+                                                -8  -10 -10
+                                                -5   0   20"/>
+                                    </asset>
+                                  </mujoco>)");
+
+  auto namefile = createTempFile(xmlDataNoStrip);
+
+  typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
+  pinocchio::Model model_m;
+  MjcfGraph::UrdfVisitor visitor(model_m);
+
+  MjcfGraph graph(visitor, "/fakeMjcf/fake.xml");
+  graph.parseGraphFromXML(namefile.name());
+
+  // Test Meshes
+  pinocchio::mjcf::details::MjcfMesh mesh = graph.mapOfMeshes.at("chasis");
+  BOOST_CHECK_EQUAL(mesh.scale, Eigen::Vector3d(0.01, 0.006, 0.0015));
+  Eigen::MatrixX3d vertices(9, 3);
+  vertices << 9, 2, 0, -10, 10, 10, 9, -2, 0, 10, 3, -10, 10, -3, -10, -8, 10, -10, -10, -10, 10,
+    -8, -10, -10, -5, 0, 20;
+  BOOST_CHECK_EQUAL(mesh.vertices.rows(), 9);
+  for (auto i = 0; i < mesh.vertices.rows(); ++i)
+  {
+    BOOST_CHECK(mesh.vertices.row(i) == vertices.row(i));
+  }
+}
+
 BOOST_AUTO_TEST_SUITE_END()