diff --git a/CHANGELOG.md b/CHANGELOG.md index d14d68601..0e755c36a 100644 --- 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 index f694d33fe..081a0cf35 100644 --- 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 index 27c85c671..b71f4a7f4 100644 --- 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>(); + 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 index 66847fcef..9cc217fa4 100644 --- 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(".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(".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(".vertex"); + if (!vertex) + { + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Only meshes with files/vertices are supported.") + } + + auto name = el.get_optional(".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 index 41b114148..25c016acc 100644 --- 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"( + + + + )"); + + 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()