From 2539bce737f4f4affc6d0e357461e595239795cb Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Thu, 2 Jan 2025 17:59:54 +0100 Subject: [PATCH 1/7] nix: clean --- flake.nix | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/flake.nix b/flake.nix index 01837fb554..3c4bc0ec45 100644 --- a/flake.nix +++ b/flake.nix @@ -20,11 +20,7 @@ devShells.default = pkgs.mkShell { inputsFrom = [ self'.packages.default ]; }; packages = { default = self'.packages.pinocchio; - pinocchio = pkgs.python3Packages.pinocchio.overrideAttrs (super: { - # avoid SIGTRAP on macos github runners - cmakeFlags = super.cmakeFlags ++ pkgs.lib.optionals pkgs.stdenv.isDarwin [ - "-DCMAKE_CTEST_ARGUMENTS=--exclude-regex;pinocchio-example-py-casadi-quadrotor-ocp" - ]; + pinocchio = pkgs.python3Packages.pinocchio.overrideAttrs { src = pkgs.lib.fileset.toSource { root = ./.; fileset = pkgs.lib.fileset.unions [ @@ -42,7 +38,7 @@ ./utils ]; }; - }); + }; }; }; }; From b18c5d9a562e1264d8d9c2588c9b76081b817f17 Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Thu, 2 Jan 2025 18:00:21 +0100 Subject: [PATCH 2/7] nix flake update --- flake.lock | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/flake.lock b/flake.lock index 0e97f2c8ea..5ebce4b9d4 100644 --- a/flake.lock +++ b/flake.lock @@ -5,11 +5,11 @@ "nixpkgs-lib": "nixpkgs-lib" }, "locked": { - "lastModified": 1726153070, - "narHash": "sha256-HO4zgY0ekfwO5bX0QH/3kJ/h4KvUDFZg8YpkNwIbg1U=", + "lastModified": 1735774679, + "narHash": "sha256-soePLBazJk0qQdDVhdbM98vYdssfs3WFedcq+raipRI=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "bcef6817a8b2aa20a5a6dbb19b43e63c5bf8619a", + "rev": "f2f7418ce0ab4a5309a4596161d154cfc877af66", "type": "github" }, "original": { @@ -20,11 +20,11 @@ }, "nixpkgs": { "locked": { - "lastModified": 1726243404, - "narHash": "sha256-sjiGsMh+1cWXb53Tecsm4skyFNag33GPbVgCdfj3n9I=", + "lastModified": 1735471104, + "narHash": "sha256-0q9NGQySwDQc7RhAV2ukfnu7Gxa5/ybJ2ANT8DQrQrs=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "345c263f2f53a3710abe117f28a5cb86d0ba4059", + "rev": "88195a94f390381c6afcdaa933c2f6ff93959cb4", "type": "github" }, "original": { @@ -36,14 +36,14 @@ }, "nixpkgs-lib": { "locked": { - "lastModified": 1725233747, - "narHash": "sha256-Ss8QWLXdr2JCBPcYChJhz4xJm+h/xjl4G0c0XlP6a74=", + "lastModified": 1735774519, + "narHash": "sha256-CewEm1o2eVAnoqb6Ml+Qi9Gg/EfNAxbRx1lANGVyoLI=", "type": "tarball", - "url": "https://github.com/NixOS/nixpkgs/archive/356624c12086a18f2ea2825fed34523d60ccc4e3.tar.gz" + "url": "https://github.com/NixOS/nixpkgs/archive/e9b51731911566bbf7e4895475a87fe06961de0b.tar.gz" }, "original": { "type": "tarball", - "url": "https://github.com/NixOS/nixpkgs/archive/356624c12086a18f2ea2825fed34523d60ccc4e3.tar.gz" + "url": "https://github.com/NixOS/nixpkgs/archive/e9b51731911566bbf7e4895475a87fe06961de0b.tar.gz" } }, "root": { From 0448a289e044b8e88238572768c6e8520cd5b4df Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Thu, 2 Jan 2025 18:02:44 +0100 Subject: [PATCH 3/7] ci: weekly update flake lock --- .github/workflows/update-flake-lock.yml | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 .github/workflows/update-flake-lock.yml diff --git a/.github/workflows/update-flake-lock.yml b/.github/workflows/update-flake-lock.yml new file mode 100644 index 0000000000..7273473461 --- /dev/null +++ b/.github/workflows/update-flake-lock.yml @@ -0,0 +1,17 @@ +name: update-flake-lock + +on: + workflow_dispatch: + schedule: + - cron: '0 1 5 * *' + +jobs: + lockfile: + runs-on: ubuntu-latest + steps: + - name: Checkout repository + uses: actions/checkout@v4 + - name: Install Nix + uses: DeterminateSystems/nix-installer-action@main + - name: Update flake.lock + uses: DeterminateSystems/update-flake-lock@main From 0e49e4bf7fd0dfe5657fe8ad8354927d1e2cf530 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Wed, 1 Jan 2025 00:33:56 +0000 Subject: [PATCH 4/7] 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 f694d33fe1..6ec4d5bb75 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 27c85c6714..b71f4a7f4a 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 66847fcef5..d21faee9ae 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 41b1141485..57c7472899 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 5/7] Add changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 34569aafc6..af3cd68a2a 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 6/7] 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 6ec4d5bb75..081a0cf35e 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 d21faee9ae..855e61928e 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 57c7472899..25c016accf 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 7/7] 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 855e61928e..9cc217fa43 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