diff --git a/.github/workflows/update-flake-lock.yml b/.github/workflows/update-flake-lock.yml new file mode 100644 index 000000000..727347346 --- /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 diff --git a/CHANGELOG.md b/CHANGELOG.md index 6c655f4e5..381b781b0 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)) - Fix mjcf parsing of keyframe qpos with newlines ([#2535](https://github.com/stack-of-tasks/pinocchio/pull/2535)) ## [3.3.1] - 2024-12-13 diff --git a/flake.lock b/flake.lock index 0e97f2c8e..5ebce4b9d 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": { diff --git a/flake.nix b/flake.nix index 01837fb55..3c4bc0ec4 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 ]; }; - }); + }; }; }; }; diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp index a47c16d5f..4960a24a6 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 3ceba2faa..553c6c72d 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -1387,4 +1387,45 @@ BOOST_AUTO_TEST_CASE(test_get_unknown_size_vector_from_stream) BOOST_CHECK(v3 == expected3); } +/// @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()