From 0e49e4bf7fd0dfe5657fe8ad8354927d1e2cf530 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Wed, 1 Jan 2025 00:33:56 +0000 Subject: [PATCH 1/4] Support vertex-based mesh definitions in MJCF --- include/pinocchio/parsers/mjcf/mjcf-graph.hpp | 2 + src/parsers/mjcf/mjcf-graph-geom.cpp | 13 +++++ src/parsers/mjcf/mjcf-graph.cpp | 51 +++++++++++++++---- unittest/mjcf.cpp | 41 +++++++++++++++ 4 files changed, 97 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp index f694d33fe..6ec4d5bb7 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 + hpp::fcl::MatrixX3s 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..d21faee9a 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -582,20 +582,51 @@ 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) + { + throw std::invalid_argument("Only meshes with files/vertices are supported"); + } + + auto name = el.get_optional(".name"); + if (!name) + { + throw std::invalid_argument("Mesh with vertices without a name is not supported"); + } + + // Parse and validate vertices + Eigen::VectorXd meshVertices = internal::getUnknownSizeVectorFromStream(*vertex); + if (meshVertices.size() % 3 != 0) + { + throw std::invalid_argument("Number of vertices is not a multiple of 3"); + } + + // Convert to 3D vertex matrix + const size_t numVertices = meshVertices.size() / 3; + hpp::fcl::MatrixX3s vertices(numVertices, 3); + + for (size_t i = 0; i < meshVertices.size(); i += 3) + { + vertices.row(i / 3) = meshVertices.segment<3>(i).transpose(); + } - mapOfMeshes.insert(std::make_pair(name, mesh)); + 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..57c747289 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)); + hpp::fcl::MatrixX3s vertices(3, 9); + 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() From bee9ea4d9e36a1d86f9c30c81c9d0d0bc55f21e9 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Wed, 1 Jan 2025 00:53:57 +0000 Subject: [PATCH 2/4] Add changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 34569aafc..af3cd68a2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,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)) +- Add parsing meshes with vertices for MJCF format ([#2537](https://github.com/stack-of-tasks/pinocchio/pull/2537)) ## [3.3.1] - 2024-12-13 From ba4a0548e591efc96164e68440c43ba8f1aced03 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Wed, 1 Jan 2025 01:17:43 +0000 Subject: [PATCH 3/4] Replace hpp::fcl::MatrixX3s with Eigen::MatrixX3d --- include/pinocchio/parsers/mjcf/mjcf-graph.hpp | 2 +- src/parsers/mjcf/mjcf-graph.cpp | 10 ++++------ unittest/mjcf.cpp | 2 +- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp index 6ec4d5bb7..081a0cf35 100644 --- a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp +++ b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp @@ -210,7 +210,7 @@ namespace pinocchio // Path to the mesh file std::string filePath; // Vertices of the mesh - hpp::fcl::MatrixX3s vertices; + Eigen::MatrixX3d vertices; }; /// @brief All informations related to a texture are stored here diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index d21faee9a..855e61928 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -617,14 +617,12 @@ namespace pinocchio } // Convert to 3D vertex matrix - const size_t numVertices = meshVertices.size() / 3; - hpp::fcl::MatrixX3s vertices(numVertices, 3); - - for (size_t i = 0; i < meshVertices.size(); i += 3) + const auto numVertices = meshVertices.size() / 3; + Eigen::MatrixX3d vertices(numVertices, 3); + for (auto i = 0; i < numVertices; ++i) { - vertices.row(i / 3) = meshVertices.segment<3>(i).transpose(); + vertices.row(i) = meshVertices.segment<3>(3 * i).transpose(); } - mesh.vertices = vertices; mapOfMeshes.insert(std::make_pair(*name, mesh)); } diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp index 57c747289..25c016acc 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -1388,7 +1388,7 @@ BOOST_AUTO_TEST_CASE(parse_mesh_with_vertices) // Test Meshes pinocchio::mjcf::details::MjcfMesh mesh = graph.mapOfMeshes.at("chasis"); BOOST_CHECK_EQUAL(mesh.scale, Eigen::Vector3d(0.01, 0.006, 0.0015)); - hpp::fcl::MatrixX3s vertices(3, 9); + 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); From 01a36dd37d4ae3d0fd0410bc22b0f6659e49fa5a Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 2 Jan 2025 21:30:29 +0000 Subject: [PATCH 4/4] Use PINOCCHIO_THROW_PRETTY rather than plain exception --- src/parsers/mjcf/mjcf-graph.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index 855e61928..9cc217fa4 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -600,20 +600,23 @@ namespace pinocchio auto vertex = el.get_optional(".vertex"); if (!vertex) { - throw std::invalid_argument("Only meshes with files/vertices are supported"); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Only meshes with files/vertices are supported.") } auto name = el.get_optional(".name"); if (!name) { - throw std::invalid_argument("Mesh with vertices without a name is not supported"); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Mesh with vertices without a name is not supported"); } // Parse and validate vertices Eigen::VectorXd meshVertices = internal::getUnknownSizeVectorFromStream(*vertex); if (meshVertices.size() % 3 != 0) { - throw std::invalid_argument("Number of vertices is not a multiple of 3"); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Number of vertices is not a multiple of 3"); } // Convert to 3D vertex matrix