From 180150dadffc8f74c745e607b1df075deefde52a Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Wed, 13 Mar 2024 15:08:17 -0700 Subject: [PATCH] MakeConvexHull now supports glTF files (#21131) We're using VTK to parse the glTF file. For VTK's parser to work, it needs the OpenGl2 rendering artifacts initialized. This required moving VTK module initialization out of `internal_render_engine_vtk.cc` and into its own compilation unit to be shared. It exercises the API in planar_scenegraph_visualizer. While the API could probably be promoted to part of the stable API, that decision is being deferred for now. It is bound as _MakeConvexHull. --- .../pydrake/geometry/geometry_py_hydro.cc | 8 ++ bindings/pydrake/geometry/test/hydro_test.py | 5 + .../systems/planar_scenegraph_visualizer.py | 24 ++--- geometry/BUILD.bazel | 21 ++++ geometry/proximity/BUILD.bazel | 7 ++ geometry/proximity/make_convex_hull_mesh.cc | 83 +++++++++++++--- geometry/proximity/make_convex_hull_mesh.h | 2 +- .../test/make_convex_hull_mesh_test.cc | 62 +++++++++++- geometry/render_vtk/BUILD.bazel | 1 + .../render_vtk/internal_render_engine_vtk.cc | 4 - geometry/test/cube_with_hole.bin | Bin 0 -> 1728 bytes geometry/test/cube_with_hole.gltf | 91 ++++++++++++++++++ geometry/vtk_opengl_init.cc | 14 +++ 13 files changed, 288 insertions(+), 34 deletions(-) create mode 100644 geometry/test/cube_with_hole.bin create mode 100644 geometry/test/cube_with_hole.gltf create mode 100644 geometry/vtk_opengl_init.cc diff --git a/bindings/pydrake/geometry/geometry_py_hydro.cc b/bindings/pydrake/geometry/geometry_py_hydro.cc index c16d556afa8b..b29c1f7759ee 100644 --- a/bindings/pydrake/geometry/geometry_py_hydro.cc +++ b/bindings/pydrake/geometry/geometry_py_hydro.cc @@ -7,6 +7,7 @@ #include "drake/bindings/pydrake/common/type_safe_index_pybind.h" #include "drake/bindings/pydrake/documentation_pybind.h" #include "drake/geometry/geometry_roles.h" +#include "drake/geometry/proximity/make_convex_hull_mesh.h" #include "drake/geometry/proximity/obj_to_surface_mesh.h" #include "drake/geometry/proximity/polygon_surface_mesh.h" #include "drake/geometry/proximity/polygon_surface_mesh_field.h" @@ -267,6 +268,13 @@ void DoScalarIndependentDefinitions(py::module m) { DefCopyAndDeepCopy(&cls); } + // MakeConvexHull + { + constexpr char internal_doc[] = "(internal use only)"; + m.def("_MakeConvexHull", &drake::geometry::internal::MakeConvexHull, + py::arg("shape"), internal_doc); + } + m.def( "ReadObjToTriangleSurfaceMesh", [](const std::string& filename, double scale) { diff --git a/bindings/pydrake/geometry/test/hydro_test.py b/bindings/pydrake/geometry/test/hydro_test.py index 3edee1eb477a..d18f9b21db31 100644 --- a/bindings/pydrake/geometry/test/hydro_test.py +++ b/bindings/pydrake/geometry/test/hydro_test.py @@ -248,3 +248,8 @@ def test_read_obj_to_surface_mesh(self): ] for i, expected in enumerate(expected_vertices): self.assertListEqual(list(vertices[i]), expected) + + def test_make_convex_hull(self): + mesh_path = FindResourceOrThrow("drake/geometry/test/quad_cube.obj") + hull = mut._MakeConvexHull(mut.Convex(mesh_path)) + self.assertIsInstance(hull, mut.PolygonSurfaceMesh) diff --git a/bindings/pydrake/systems/planar_scenegraph_visualizer.py b/bindings/pydrake/systems/planar_scenegraph_visualizer.py index 2f8289ce7fc7..cf711e40c864 100644 --- a/bindings/pydrake/systems/planar_scenegraph_visualizer.py +++ b/bindings/pydrake/systems/planar_scenegraph_visualizer.py @@ -11,6 +11,7 @@ from pydrake.common.deprecation import _warn_deprecated from pydrake.common.value import Value from pydrake.geometry import ( + _MakeConvexHull, Box, Convex, Cylinder, @@ -244,9 +245,11 @@ def _build_body_patches(self, use_random_colors, for pt in sample_pts]) elif isinstance(shape, (Mesh, Convex)): + # Legacy behavior for looking for a .obj when the extension + # is not recognized. filename = shape.filename() base, ext = os.path.splitext(filename) - if (ext.lower() != ".obj" + if (ext.lower() not in [".obj", ".vtk", ".gltf"] and substitute_collocated_mesh_files): # Check for a co-located .obj file (case insensitive). for f in glob.glob(base + '.*'): @@ -261,17 +264,14 @@ def _build_body_patches(self, use_random_colors, if not os.path.exists(filename): raise FileNotFoundError(errno.ENOENT, os.strerror( errno.ENOENT), filename) - # Get mesh scaling. - scale = shape.scale() - mesh = ReadObjToTriangleSurfaceMesh(filename, scale) - patch_G = np.vstack(mesh.vertices()) - - # Only store the vertices of the (3D) convex hull of the - # mesh, as any interior vertices will still be interior - # vertices after projection, and will therefore be removed - # in _update_body_fill_verts(). - vpoly = optimization.VPolytope(patch_G.T) - patch_G = vpoly.GetMinimalRepresentation().vertices() + temp_mesh = Mesh(filename, shape.scale()) + # TODO(21125): When Convex shape *always* has a + # convex hull available, use it instead of recomputing it + # here. + convex_hull = _MakeConvexHull(temp_mesh) + patch_G = np.empty((3, convex_hull.num_vertices())) + for i in range(convex_hull.num_vertices()): + patch_G[:, i] = convex_hull.vertex(i) elif isinstance(shape, HalfSpace): # For a half space, we'll simply create a large box with diff --git a/geometry/BUILD.bazel b/geometry/BUILD.bazel index a7e56b48e910..3814134e2868 100644 --- a/geometry/BUILD.bazel +++ b/geometry/BUILD.bazel @@ -679,6 +679,18 @@ drake_cc_library( ], ) +drake_cc_library( + name = "vtk_opengl_init", + srcs = ["vtk_opengl_init.cc"], + internal = True, + visibility = ["//geometry:__subpackages__"], + deps = [ + "@vtk_internal//:vtkCommonCore", + "@vtk_internal//:vtkRenderingOpenGL2", + ], + alwayslink = True, +) + drake_cc_googletest( name = "meshcat_visualizer_test", data = [ @@ -699,6 +711,15 @@ drake_cc_googletest( # ----------------------------------------------------- +filegroup( + name = "test_gltf_files", + testonly = 1, + srcs = [ + "test/cube_with_hole.bin", + "test/cube_with_hole.gltf", + ], +) + filegroup( name = "test_obj_files", testonly = 1, diff --git a/geometry/proximity/BUILD.bazel b/geometry/proximity/BUILD.bazel index ec0349bf5dcb..e9f10e319dcb 100644 --- a/geometry/proximity/BUILD.bazel +++ b/geometry/proximity/BUILD.bazel @@ -482,7 +482,12 @@ drake_cc_library( "//common:essential", "//common:find_resource", "//geometry:read_obj", + "//geometry:vtk_opengl_init", + "//math:geometric_transform", "@qhull_internal//:qhull", + "@vtk_internal//:vtkCommonCore", + "@vtk_internal//:vtkIOImport", + "@vtk_internal//:vtkRenderingCore", ], ) @@ -1237,6 +1242,7 @@ drake_cc_googletest( drake_cc_googletest( name = "make_convex_hull_mesh_test", data = [ + "//geometry:test_gltf_files", "//geometry:test_obj_files", "//geometry:test_vtk_files", "//geometry/render:test_models", @@ -1246,6 +1252,7 @@ drake_cc_googletest( "//common:find_resource", "//common:temp_directory", "//common/test_utilities:expect_throws_message", + "@nlohmann_internal//:nlohmann", ], ) diff --git a/geometry/proximity/make_convex_hull_mesh.cc b/geometry/proximity/make_convex_hull_mesh.cc index 9027367116df..cc892dc774ae 100644 --- a/geometry/proximity/make_convex_hull_mesh.cc +++ b/geometry/proximity/make_convex_hull_mesh.cc @@ -6,10 +6,16 @@ #include #include +// To ease build system upkeep, we annotate VTK includes with their deps. #include #include #include #include +#include // vtkIOImport +#include // vtkCommonCore +#include // vtkCommonMath +#include // vtkCommonDataModel +#include // vtkRenderingCore #include "drake/common/find_resource.h" #include "drake/common/fmt_eigen.h" @@ -18,6 +24,8 @@ #include "drake/geometry/proximity/volume_mesh.h" #include "drake/geometry/proximity/vtk_to_volume_mesh.h" #include "drake/geometry/read_obj.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/rotation_matrix.h" namespace drake { namespace geometry { @@ -25,6 +33,8 @@ namespace internal { namespace { using Eigen::Vector3d; +using math::RigidTransformd; +using math::RotationMatrixd; /* Used for ordering polygon vertices according to their angular distance from a reference direction. See OrderPolyVertices(). */ @@ -100,8 +110,8 @@ struct VertexCloud { Vector3d interior_point; }; -/* Simply reads the vertices from an OBJ or VTK file referred to by either a - Mesh or Convex shape. Supports getting the vertices for MakeConvexHull. +/* Simply reads the vertices from an OBJ, VTK or glTF file referred to by either + a Mesh or Convex shape. Supports getting the vertices for MakeConvexHull. When calling reify, the user data should be an instance of VertexCloud. The reifier will write the results into that instance. */ @@ -129,17 +139,20 @@ class VertexReader final : public ShapeReifier { ReadObjVertices(filename, scale, &cloud.vertices); } else if (extension == ".vtk") { ReadVtkVertices(filename, scale, &cloud.vertices); + } else if (extension == ".gltf") { + ReadGltfVertices(filename, scale, &cloud.vertices); } else { throw std::runtime_error(fmt::format( - "MakeConvexHull only applies to obj and vtk meshes; given file: {}.", + "MakeConvexHull only applies to obj, vtk, and gltf meshes; " + "given file: {}.", filename)); } if (cloud.vertices.size() < 3) { - throw std::runtime_error( - fmt::format("MakeConvexHull() cannot be used on a mesh with fewer " - "than three vertices; found {} vertices in file: {}.", - cloud.vertices.size(), filename)); + throw std::runtime_error(fmt::format( + "MakeConvexHull() cannot be used on a mesh with fewer than three " + "vertices; found {} vertices in file: {}.", + cloud.vertices.size(), filename)); } /* Characterizes planarity. */ @@ -180,6 +193,51 @@ class VertexReader final : public ShapeReifier { *vertices = volume_mesh.vertices(); } + // Multiplies the position vector p_AQ by the transform T_BA, returning p_BQ. + Vector3d VtkMultiply(vtkMatrix4x4* T_BA, const Vector3d& p_AQ) { + double p_in[] = {p_AQ.x(), p_AQ.y(), p_AQ.z(), 1}; + double p_out[4]; + T_BA->MultiplyPoint(p_in, p_out); + return Vector3d(p_out); + } + + void ReadGltfVertices(std::string_view filename, double scale, + std::vector* vertices) { + vtkNew importer; + importer->SetFileName(filename.data()); + importer->Update(); + + auto* renderer = importer->GetRenderer(); + DRAKE_DEMAND(renderer != nullptr); + + if (renderer->VisibleActorCount() == 0) { + throw std::runtime_error(fmt::format( + "MakeConvexHull() found no vertices in the file '{}'.", filename)); + } + + // The relative transform from the file's frame F to the geometry's frame G. + // (rotation from y-up to z-up). The scale is handled separately. + const RigidTransformd X_GF(RotationMatrixd::MakeXRotation(M_PI / 2)); + + auto* actors = renderer->GetActors(); + actors->InitTraversal(); + while (vtkActor* actor = actors->GetNextActor()) { + // 1. Extract PolyData from actor. + auto* poly_data = + dynamic_cast(actor->GetMapper()->GetInput()); + DRAKE_DEMAND(poly_data != nullptr); + // 2. For each vertex, transform it to the file frame (based on the + // actor's user transform), and then into the geometry frame using the + // scale and rotation. + vtkMatrix4x4* T_FA = actor->GetUserMatrix(); + for (vtkIdType vi = 0; vi < poly_data->GetNumberOfPoints(); ++vi) { + const Vector3d p_AV(poly_data->GetPoint(vi)); + const Vector3d p_FV = VtkMultiply(T_FA, p_AV); + vertices->emplace_back((X_GF * p_FV) * scale); + } + } + } + /* Given a set of vertices, attempts to find a plane that reliably spans three vertices. The normal is defined as n = a x b / |a x b| where a = v[j] - v[0] and @@ -205,7 +263,8 @@ class VertexReader final : public ShapeReifier { if (v == ssize(vertices)) { throw std::runtime_error( fmt::format("MakeConvexHull failed because all vertices in the mesh " - "were within a sphere with radius 1e-12 for file: {}.", + "were within a " + "sphere with radius 1e-12 for file: {}.", filename)); } a.normalize(); @@ -221,10 +280,10 @@ class VertexReader final : public ShapeReifier { } if (v == ssize(vertices)) { - throw std::runtime_error( - fmt::format("MakeConvexHull failed because all vertices in the mesh " - "appear to be co-linear for file: {}.", - filename)); + throw std::runtime_error(fmt::format( + "MakeConvexHull failed because all vertices in the mesh appear to be " + "co-linear for file: {}.", + filename)); } return n_candidate.normalized(); } diff --git a/geometry/proximity/make_convex_hull_mesh.h b/geometry/proximity/make_convex_hull_mesh.h index 3389cabc0e89..962241b78ccd 100644 --- a/geometry/proximity/make_convex_hull_mesh.h +++ b/geometry/proximity/make_convex_hull_mesh.h @@ -13,7 +13,7 @@ namespace internal { @param shape The shape to bound. @throws if `shape` is not an instance of Mesh or Convex. - @throws if `shape` references anything but an .obj or .vtk volume mesh. + @throws if `shape` references anything but an .obj, .vtk volume mesh, or .gltf. @throws if the referenced mesh data is degenerate (insufficient number of vertices, co-linear or coincident vertices, etc.) All of the vertices lying on a plane is *not* degenerate. diff --git a/geometry/proximity/test/make_convex_hull_mesh_test.cc b/geometry/proximity/test/make_convex_hull_mesh_test.cc index adba3b616bae..3b58993e65e6 100644 --- a/geometry/proximity/test/make_convex_hull_mesh_test.cc +++ b/geometry/proximity/test/make_convex_hull_mesh_test.cc @@ -6,6 +6,7 @@ #include #include +#include #include "drake/common/find_resource.h" #include "drake/common/fmt_eigen.h" @@ -22,6 +23,8 @@ using Eigen::Vector3d; using PolyMesh = PolygonSurfaceMesh; using std::vector; +namespace fs = std::filesystem; + /* To compare polygon meshes we will produce "canonical" representations. The canonical mesh is the same manifold as the input mesh but has the following properties: @@ -237,7 +240,7 @@ GTEST_TEST(MakeConvexHullMeshTest, RejectBadShapes) { DRAKE_EXPECT_THROWS_MESSAGE(MakeConvexHull(Sphere(1)), ".* only applies .*"); DRAKE_EXPECT_THROWS_MESSAGE( MakeConvexHull(Mesh("thing.stl", 1)), - ".* only applies to obj and vtk meshes.*thing.stl.*"); + ".* only applies to obj, vtk, and gltf.*thing.stl.*"); // TODO(SeanCurtis-TRI): Figure out how to trigger a qhull error that isn't // prevented by Drake code do kcov can be happy about that code being @@ -330,10 +333,59 @@ GTEST_TEST(MakeConvexHullMeshTest, VolumeMesh) { MeshesAreEquivalent(dut, expected, 1e-14); } +/* A reality check that it also works on glTF mesh files. */ +GTEST_TEST(MakeConvexHullMeshTest, GltfMesh) { + const double scale = 2.0; + /* The glTF's bin file contains a cube with a hole in the center. We'll + apply a transform to the glTF node so it's no longer centered so we can + confirm that we're handling the y-up vs z-up transformation. + + However, we're also applying a Drake scale factor to it. So, the expected + cube is the unit cube, first offset and then scaled. So, we'll construct + that expected cube here. */ + const PolyMesh bin_cube = MakeCube(1); + const Vector3d p_WC(1, 2, 3); + vector face_data = bin_cube.face_data(); + vector vertices; + for (int vi = 0; vi < bin_cube.num_vertices(); ++vi) { + vertices.push_back((bin_cube.vertex(vi) + p_WC) * scale); + } + const PolyMesh expected(std::move(face_data), std::move(vertices)); + + /* Create a custom version of cube_with_hole.gltf that is displaced by + p_WC. Simply copy the .bin and write out a modified .gltf file. */ + const fs::path dir_path(temp_directory()); + + const fs::path bin_source = + FindResourceOrThrow("drake/geometry/test/cube_with_hole.bin"); + const fs::path bin_target = dir_path / bin_source.filename(); + fs::copy_file(bin_source, bin_target); + + const fs::path gltf_source = + FindResourceOrThrow("drake/geometry/test/cube_with_hole.gltf"); + const fs::path gltf_target = dir_path / gltf_source.filename(); + { + std::ifstream in_gltf(gltf_source); + DRAKE_DEMAND(in_gltf.is_open()); + nlohmann::json cube = nlohmann::json::parse(in_gltf); + DRAKE_DEMAND(cube["nodes"].size() == 1); + // p_WC is expressed in Drake's z-up world. So, when we set it in the file, + // it needs to be glTF's y-up world. + cube["nodes"][0]["translation"] = {p_WC.x(), p_WC.z(), -p_WC.y()}; + std::ofstream out_gltf(gltf_target); + DRAKE_DEMAND(out_gltf.is_open()); + out_gltf << cube; + } + + const PolyMesh dut = MakeConvexHull(Mesh(gltf_target, scale)); + + MeshesAreEquivalent(dut, expected, 1e-14); +} + /* Tests what happens when a mesh that is strictly a 2D surface. */ GTEST_TEST(MakeConvexHullMeshTest, PlanarSurface) { - const std::filesystem::path dir_path(temp_directory()); - const std::filesystem::path obj_path = dir_path / "one_square.obj"; + const fs::path dir_path(temp_directory()); + const fs::path obj_path = dir_path / "one_square.obj"; { std::ofstream file(obj_path); DRAKE_DEMAND(file.is_open()); @@ -385,10 +437,10 @@ f 1//1 2//1 4//1 3//1 // There are some mesh cases that we simply throw; they are and should be // considered degenerate. GTEST_TEST(MakeConvexHullMeshTest, DegenerateMeshes) { - const std::filesystem::path dir_path(temp_directory()); + const fs::path dir_path(temp_directory()); auto make_obj = [&dir_path](std::string_view name, std::string_view contents) { - std::filesystem::path obj_path = dir_path / name; + fs::path obj_path = dir_path / name; std::ofstream file(obj_path); DRAKE_DEMAND(file.is_open()); file << contents; diff --git a/geometry/render_vtk/BUILD.bazel b/geometry/render_vtk/BUILD.bazel index 712a107e91b4..1ac72af58125 100644 --- a/geometry/render_vtk/BUILD.bazel +++ b/geometry/render_vtk/BUILD.bazel @@ -58,6 +58,7 @@ drake_cc_library( ":internal_render_engine_vtk_base", ":internal_vtk_util", "//common", + "//geometry:vtk_opengl_init", "//geometry/render:render_engine", "//geometry/render/shaders:depth_shaders", "//geometry/render_vtk:render_engine_vtk_params", diff --git a/geometry/render_vtk/internal_render_engine_vtk.cc b/geometry/render_vtk/internal_render_engine_vtk.cc index b92410e81c6e..6adf80f618f8 100644 --- a/geometry/render_vtk/internal_render_engine_vtk.cc +++ b/geometry/render_vtk/internal_render_engine_vtk.cc @@ -9,7 +9,6 @@ #include // To ease build system upkeep, we annotate VTK includes with their deps. -#include // vtkCommonCore #include // vtkRenderingCore #include // vtkRenderingOpenGL2 #include // vtkFiltersSources @@ -50,9 +49,6 @@ #include "drake/math/rotation_matrix.h" #include "drake/systems/sensors/vtk_diagnostic_event_observer.h" -// This enables VTK's OpenGL2 infrastructure. -VTK_MODULE_INIT(vtkRenderingOpenGL2) - namespace drake { namespace geometry { namespace render_vtk { diff --git a/geometry/test/cube_with_hole.bin b/geometry/test/cube_with_hole.bin new file mode 100644 index 0000000000000000000000000000000000000000..e3bfa0342d7078631e53ab441720053c0e841b2a GIT binary patch literal 1728 zcmbV}Sy~i95QKks6csnz6>$S~6hRq5aq1SFf)tdzEf=*XHIDmkdxW+R{}*le#n*Scc;x-<$??j={n_!x?shWw zL0z5=e6GB>JOTO_J@AMBp+q8Ld#pu(0nAci$hRXlm zXQQ`Y)~|SlUSCt^!oSSf;a~K=z< zd+zg`CE*gUxXe8kg^RpnmPf1!*Z9Oat}+k~_{vjmvMJo;4Uf4)5+=TIn+ptuLq^=- Y63fD6HaO2M=7sZI;Vw^D5H9e5KRKP*zyJUM literal 0 HcmV?d00001 diff --git a/geometry/test/cube_with_hole.gltf b/geometry/test/cube_with_hole.gltf new file mode 100644 index 000000000000..46423f717734 --- /dev/null +++ b/geometry/test/cube_with_hole.gltf @@ -0,0 +1,91 @@ +{ + "asset":{ + "generator":"Khronos glTF Blender I/O v4.0.44", + "version":"2.0" + }, + "scene":0, + "scenes":[ + { + "name":"Scene", + "nodes":[ + 0 + ] + } + ], + "nodes":[ + { + "mesh":0, + "name":"Cube" + } + ], + "meshes":[ + { + "name":"Cube.002", + "primitives":[ + { + "attributes":{ + "POSITION":0, + "NORMAL":1 + }, + "indices":2 + } + ] + } + ], + "accessors":[ + { + "bufferView":0, + "componentType":5126, + "count":64, + "max":[ + 1, + 1, + 1 + ], + "min":[ + -1, + -1, + -1 + ], + "type":"VEC3" + }, + { + "bufferView":1, + "componentType":5126, + "count":64, + "type":"VEC3" + }, + { + "bufferView":2, + "componentType":5123, + "count":96, + "type":"SCALAR" + } + ], + "bufferViews":[ + { + "buffer":0, + "byteLength":768, + "byteOffset":0, + "target":34962 + }, + { + "buffer":0, + "byteLength":768, + "byteOffset":768, + "target":34962 + }, + { + "buffer":0, + "byteLength":192, + "byteOffset":1536, + "target":34963 + } + ], + "buffers":[ + { + "byteLength":1728, + "uri":"cube_with_hole.bin" + } + ] +} diff --git a/geometry/vtk_opengl_init.cc b/geometry/vtk_opengl_init.cc new file mode 100644 index 000000000000..bc148e65c2f8 --- /dev/null +++ b/geometry/vtk_opengl_init.cc @@ -0,0 +1,14 @@ + +// To ease build system upkeep, we annotate VTK includes with their deps. +#include // vtkCommonCore +#include // vtkRenderingOpenGL2 + +// There are some VTK operations that depend on VTK's OpenGL module being +// initialized. In addition to the obvious RenderEngineVtk, the less obvious +// MakeConvexHull() also depends (VTK parses glTF files directly into OpenGL +// constructs). +// +// If a translation unit uses VTK *and* requires OpenGL to be initialized, it +// should declare //geometry:vtk_opengl_init as a dependency. + +VTK_MODULE_INIT(vtkRenderingOpenGL2)