From a5a6379f83b21e9ac766a394c468b9c12341e181 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 4 Aug 2025 19:34:35 -0700 Subject: [PATCH] Add built-in cone mesh type. Also delete `prism` type since it is subsumed by cone. PiperOrigin-RevId: 790992343 Change-Id: I7a9c6f2a4377489173acd64b23086c748d2cee8e --- doc/XMLreference.rst | 12 ++- doc/includes/references.h | 1 - include/mujoco/mjspec.h | 1 - python/mujoco/introspect/enums.py | 9 +-- python/mujoco/specs.cc | 17 ++--- python/mujoco/specs_test.py | 17 ++++- src/user/user_api.cc | 107 ++++++++++++++------------- src/user/user_mesh.cc | 77 +++++++++++++++---- src/user/user_objects.h | 2 +- src/xml/xml_native_reader.cc | 3 +- test/user/testdata/makemesh.xml | 19 +++-- test/xml/xml_native_reader_test.cc | 4 +- unity/Runtime/Bindings/MjBindings.cs | 9 +-- 13 files changed, 167 insertions(+), 111 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index fb94d1f0..5500d7cd 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1340,15 +1340,13 @@ The full list of processing steps applied by the compiler to each mesh is as fol **subdivision**: integer in [0-10]: The number of subdivisions to apply to the pyramid. - :at-val:`prism` (nvert) - An extruded regular unit polygon. - - **nvert**: integer >= 3: The number vertices in the polygon. - - :at-val:`cone` (nvert) - The convex hull of a regular unit polygon and the vertex (0, 0, 1). + :at-val:`cone` (nvert, radius) + The convex hull of a regular unit polygon at z = -1 and a unit polygon with the given radiusat z = 1. + If radius is 1, the mesh a prism. If radius is 0, only a single vertex is place at (0, 0, 1) and the mesh is a + discrete cone. If radius is positive, the mesh is a truncated discrete cone. **nvert**: integer >= 3: The number vertices in the polygon. + |br| **radius**: real in [0, 1]: The radius of the top face. :at-val:`torus` (radius, resolution) A torus with major radius of 1 and given minor radius. diff --git a/doc/includes/references.h b/doc/includes/references.h index 28a552a0..4b7e2e70 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1788,7 +1788,6 @@ typedef enum mjtMeshBuiltin_ { // type of built-in procedural mesh mjMESH_BUILTIN_NONE = 0, // no built-in mesh mjMESH_BUILTIN_SPHERE, // sphere mjMESH_BUILTIN_HEMISPHERE, // hemisphere - mjMESH_BUILTIN_PRISM, // prism mjMESH_BUILTIN_CONE, // cone mjMESH_BUILTIN_TORUS, // torus mjMESH_BUILTIN_WEDGE, // wedge diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index c5b70fe1..e0c856cf 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -74,7 +74,6 @@ typedef enum mjtMeshBuiltin_ { // type of built-in procedural mesh mjMESH_BUILTIN_NONE = 0, // no built-in mesh mjMESH_BUILTIN_SPHERE, // sphere mjMESH_BUILTIN_HEMISPHERE, // hemisphere - mjMESH_BUILTIN_PRISM, // prism mjMESH_BUILTIN_CONE, // cone mjMESH_BUILTIN_TORUS, // torus mjMESH_BUILTIN_WEDGE, // wedge diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index 391d69e4..72905fb6 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -800,11 +800,10 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjMESH_BUILTIN_NONE', 0), ('mjMESH_BUILTIN_SPHERE', 1), ('mjMESH_BUILTIN_HEMISPHERE', 2), - ('mjMESH_BUILTIN_PRISM', 3), - ('mjMESH_BUILTIN_CONE', 4), - ('mjMESH_BUILTIN_TORUS', 5), - ('mjMESH_BUILTIN_WEDGE', 6), - ('mjMESH_BUILTIN_PLATE', 7), + ('mjMESH_BUILTIN_CONE', 3), + ('mjMESH_BUILTIN_TORUS', 4), + ('mjMESH_BUILTIN_WEDGE', 5), + ('mjMESH_BUILTIN_PLATE', 6), ]), )), ('mjtBuiltin', diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 9056bb7d..9953d212 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1003,7 +1003,7 @@ PYBIND11_MODULE(_specs, m) { py::return_value_policy::reference_internal); mjsMesh.def( "make_wedge", - [](raw::MjsMesh* self, std::array& resolution, double radius, + [](raw::MjsMesh* self, std::array& resolution, std::array& fov, double gamma) { double params[5] = {static_cast(resolution[0]), static_cast(resolution[1]), fov[0], fov[1], @@ -1011,21 +1011,18 @@ PYBIND11_MODULE(_specs, m) { if (mjs_makeMesh(self, mjMESH_BUILTIN_WEDGE, params, 5)) { throw pybind11::value_error(mjs_getError(mjs_getSpec(self->element))); } - self->scale[0] = radius; - self->scale[1] = radius; - self->scale[2] = radius; }, - py::arg("resolution") = std::array{0, 0}, py::arg("radius"), + py::arg("resolution") = std::array{0, 0}, py::arg("fov") = std::array{0, 0}, py::arg("gamma") = 0); mjsMesh.def( - "make_prism", - [](raw::MjsMesh* self, int nedge) { - double params[1] = {static_cast(nedge)}; - if (mjs_makeMesh(self, mjMESH_BUILTIN_PRISM, params, 1)) { + "make_cone", + [](raw::MjsMesh* self, int nedge, double radius) { + double params[2] = {static_cast(nedge), radius}; + if (mjs_makeMesh(self, mjMESH_BUILTIN_CONE, params, 2)) { throw pybind11::value_error(mjs_getError(mjs_getSpec(self->element))); } }, - py::arg("nedge")); + py::arg("nedge"), py::arg("radius")); mjsMesh.def( "make_plate", [](raw::MjsMesh* self, std::array& resolution) { diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index b5982d88..e1c728a1 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -486,12 +486,21 @@ class SpecsTest(absltest.TestCase): def test_make_mesh(self): spec = mujoco.MjSpec() + mesh = spec.add_mesh(name='wedge') - mesh.make_wedge(resolution=[25, 25], radius=.1, fov=[90, 45], gamma=0) + mesh.make_wedge(resolution=[25, 25], fov=[90, 45], gamma=0) + + mesh = spec.add_mesh(name='prism') + mesh.make_cone(nedge=5, radius=1) + + mesh = spec.add_mesh(name='cone') + mesh.make_cone(nedge=6, radius=0) + model = spec.compile() - self.assertEqual(model.nmesh, 1) - self.assertEqual(model.nmeshvert, 25 * 25) - np.testing.assert_array_equal(model.mesh_scale[0], [0.1, 0.1, 0.1]) + self.assertEqual(model.nmesh, 3) + self.assertEqual(model.mesh_vertnum[0], 25 * 25) + self.assertEqual(model.mesh_vertnum[1], 10) + self.assertEqual(model.mesh_vertnum[2], 7) def test_compile_errors_with_line_info(self): spec = mujoco.MjSpec() diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 24f181d5..92ee8030 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -505,62 +505,65 @@ mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* defspec) { int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int nparams) { mjCMesh* meshC = static_cast(mesh->element); mjCModel* m = meshC->model; - if (builtin == mjMESH_BUILTIN_WEDGE) { - if (nparams != 5) { - m->SetError(mjCError(0, "Wedge builtin mesh types require 5 parameters")); - return -1; + switch (builtin) { + case mjMESH_BUILTIN_WEDGE: { + if (nparams != 5) { + m->SetError(mjCError(0, "Wedge builtin mesh types require 5 parameters")); + return -1; + } + int resolution[2] = {static_cast(params[0]), + static_cast(params[1])}; + double fov[2] = {params[2], params[3]}; + double gamma = params[4]; + if (fov[0] <= 0 || fov[0] > 180) { + m->SetError(mjCError(0, "fov[0] must be a float between (0, 180] degrees")); + return -1; + } + if (fov[1] <= 0 || fov[1] > 90) { + m->SetError(mjCError(0, "`fov[1]` must be a float between (0, 90] degrees")); + return -1; + } + if (resolution[0] <= 0 || resolution[1] <= 0) { + m->SetError(mjCError(0, "Horizontal and vertical resolutions must be positive")); + return -1; + } + if (gamma < 0 || gamma > 1) { + m->SetError(mjCError(0, "`gamma` must be a nonnegative float between [0, 1]")); + return -1; + } + meshC->MakeWedge(resolution, fov, gamma); + return 0; } - int resolution[2] = {static_cast(params[0]), - static_cast(params[1])}; - double fov[2] = {params[2], params[3]}; - double gamma = params[4]; - if (fov[0] <= 0 || fov[0] > 180) { - m->SetError( - mjCError(0, "fov[0] must be a float between (0, 180] degrees")); - return -1; + + case mjMESH_BUILTIN_PLATE: { + if (nparams != 2) { + m->SetError(mjCError(0, "Plate builtin mesh type requires 2 parameters")); + return -1; + } + int resolution[2] = {static_cast(params[0]), + static_cast(params[1])}; + if (resolution[0] <= 0 || resolution[1] <= 0) { + m->SetError(mjCError(0, "Horizontal and vertical resolutions must be positive")); + return -1; + } + meshC->MakeRect(resolution); + return 0; } - if (fov[1] <= 0 || fov[1] > 90) { - m->SetError( - mjCError(0, "`fov[1]` must be a float between (0, 90] degrees")); - return -1; + + case mjMESH_BUILTIN_CONE: { + if (nparams != 2) { + m->SetError(mjCError(0, "Cone mesh type requires 2 parameters")); + return -1; + } + int nedge = static_cast(params[0]); + meshC->MakeCone(nedge, params[1]); + return 0; } - if (resolution[0] <= 0 || resolution[1] <= 0) { - m->SetError( - mjCError(0, "Horizontal and vertical resolutions must be positive")); - return -1; - } - if (gamma < 0 || gamma > 1) { - m->SetError( - mjCError(0, "`gamma` must be a nonnegative float between [0, 1]")); - return -1; - } - meshC->MakeWedge(resolution, fov, gamma); - return 0; - } else if (builtin == mjMESH_BUILTIN_PLATE) { - if (nparams != 2) { - m->SetError(mjCError(0, "Plate builtin mesh type requires 2 parameters")); - return -1; - } - int resolution[2] = {static_cast(params[0]), - static_cast(params[1])}; - if (resolution[0] <= 0 || resolution[1] <= 0) { - m->SetError( - mjCError(0, "Horizontal and vertical resolutions must be positive")); - return -1; - } - meshC->MakeRect(resolution); - return 0; - } else if (builtin == mjMESH_BUILTIN_PRISM) { - if (nparams != 1) { - m->SetError(mjCError(0, "Prism mesh type requires 1 parameter")); - return -1; - } - int nedge = static_cast(params[0]); - meshC->MakePrism(nedge); - return 0; + + default: + m->SetError(mjCError(0, "Unsupported mesh type")); + return 1; } - m->SetError(mjCError(0, "Unsupported mesh type")); - return 1; } // add pair to model diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 745e9c5a..c33024b3 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -184,6 +184,8 @@ static void ReadFromBuffer(T* dst, const char* src) { std::memcpy(dst, src, sizeof(T)); } + + //------------------ class mjCMesh implementation -------------------------------------------------- mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) { @@ -432,6 +434,8 @@ void mjCMesh::LoadSDF() { delete[] field; } + + void mjCMesh::CacheMesh(mjCCache* cache, const mjResource* resource) { if (cache == nullptr) return; @@ -521,6 +525,8 @@ struct VertexKey { } // namespace + + // convert vertices to double precision and remove repeated vertices if requested void mjCMesh::ProcessVertices(const std::vector& vert, bool remove_repeated) { vert_.clear(); @@ -622,6 +628,8 @@ bool mjCMesh::IsMSH() const { return content_type_ == "model/vnd.mujoco.msh"; } + + // load mesh from resource; throw error on failure void mjCMesh::LoadFromResource(mjResource* resource, bool remove_repeated) { // set content type from resource name @@ -645,6 +653,8 @@ void mjCMesh::LoadFromResource(mjResource* resource, bool remove_repeated) { } } + + // compiler wrapper void mjCMesh::Compile(const mjVFS* vfs) { try { @@ -658,6 +668,8 @@ void mjCMesh::Compile(const mjVFS* vfs) { } } + + // compiler void mjCMesh::TryCompile(const mjVFS* vfs) { bool fromCache = false; @@ -1106,6 +1118,8 @@ void mjCMesh::LoadOBJ(mjResource* resource, bool remove_repeated) { ProcessVertices(attrib.vertices, remove_repeated); } + + // load mesh from cached asset, return true on success bool mjCMesh::LoadCachedMesh(mjCCache *cache, const mjResource* resource) { auto process_mesh = [&](const void* data) { @@ -1177,6 +1191,8 @@ bool mjCMesh::LoadCachedMesh(mjCCache *cache, const mjResource* resource) { return cache->PopulateData(resource, process_mesh); } + + // load STL binary mesh void mjCMesh::LoadSTL(mjResource* resource) { bool righthand = scale[0] * scale[1] * scale[2] > 0; @@ -1339,6 +1355,8 @@ void mjCMesh::LoadMSH(mjResource* resource, bool remove_repeated) { ProcessVertices(vert, remove_repeated); } + + // compute the volume and center-of-mass of the mesh given the face centroid double mjCMesh::ComputeVolume(double CoM[3], const double facecen[3]) const { double normal[3], center[3], total_volume = 0; @@ -1376,6 +1394,8 @@ double mjCMesh::ComputeVolume(double CoM[3], const double facecen[3]) const { return total_volume; } + + // compute the surface area and center-of-mass of the mesh given the face centroid double mjCMesh::ComputeSurfaceArea(double CoM[3], const double facecen[3]) const { double surface = 0; @@ -1402,6 +1422,8 @@ double mjCMesh::ComputeSurfaceArea(double CoM[3], const double facecen[3]) const return surface; } + + // apply transformations void mjCMesh::ApplyTransformations() { // translate @@ -1470,6 +1492,8 @@ void mjCMesh::ApplyTransformations() { } } + + // find centroid of faces, return total area double mjCMesh::ComputeFaceCentroid(double facecen[3]) const { double total_area = 0; @@ -1496,6 +1520,8 @@ double mjCMesh::ComputeFaceCentroid(double facecen[3]) const { return total_area; } + + void mjCMesh::Process() { // create half-edge structure (if mesh was in XML) if (halfedge_.empty()) { @@ -1737,6 +1763,7 @@ double mjCMesh::ComputeInertia(double inert[6], const double CoM[3]) const { } + void mjCMesh::Rotate(double quat[4]) { // rotate vertices and normals of mesh by quaternion double neg[4] = {quat[0], -quat[1], -quat[2], -quat[3]}; @@ -1804,17 +1831,20 @@ void mjCMesh::CheckInitialMesh() const { -// get inertia pointer +// return inertia pointer double* mjCMesh::GetInertiaBoxPtr() { return boxsz_; } + +// return volume or surface area double mjCMesh::GetVolumeRef() const { return (inertia == mjMESH_INERTIA_SHELL) ? surface_ : volume_; } + // make graph describing convex hull void mjCMesh::MakeGraph() { int adr, ok, curlong, totlong, exitcode; @@ -1999,6 +2029,8 @@ void mjCMesh::MakeGraph() { } } + + // copy graph into face data void mjCMesh::CopyGraph() { // only if face data is missing @@ -2022,6 +2054,8 @@ void mjCMesh::CopyGraph() { } } + + // make a mesh of a spherical wedge void mjCMesh::MakeWedge(int resolution[2], double fov[2], double gamma) { std::vector x_edges(resolution[0] + 1, 0); @@ -2047,6 +2081,8 @@ void mjCMesh::MakeWedge(int resolution[2], double fov[2], double gamma) { 9 * resolution[0] * resolution[1]); } + + // make a mesh of a rectangle void mjCMesh::MakeRect(int resolution[2]) { std::vector x_edges(resolution[0] + 1, 0); @@ -2089,23 +2125,36 @@ void mjCMesh::MakeRect(int resolution[2]) { 6 * (resolution[0] - 1) * (resolution[1] - 1)); } -// make a mesh of a prism -void mjCMesh::MakePrism(int nedge) { - int layer = 2; - std::vector uservert(3 * nedge * layer, 0); + +// make a mesh of a generalized discrete cone +void mjCMesh::MakeCone(int nedge, double radius) { + int n = 3 * (nedge + (radius > 0 ? nedge : 1)); + std::vector uservert(n, 0); + + // bottom face for (int i = 0; i < nedge; i++) { - for (int j = 0; j < layer; j++) { - int v = i * layer + j; - uservert[3 * v + 0] = std::cos(2 * i * mjPI / nedge); - uservert[3 * v + 1] = std::sin(2 * i * mjPI / nedge); - uservert[3 * v + 2] = -1 + 2 * j / (layer - 1); - } + uservert[3 * i + 0] = std::cos(2 * i * mjPI / nedge); + uservert[3 * i + 1] = std::sin(2 * i * mjPI / nedge); + uservert[3 * i + 2] = -1; } - mjs_setFloat(spec.uservert, uservert.data(), 3 * nedge * layer); + // top face or single point + if (radius > 0) { + for (int i = nedge; i < 2 * nedge; i++) { + uservert[3 * i + 0] = radius * std::cos(2 * i * mjPI / nedge); + uservert[3 * i + 1] = radius * std::sin(2 * i * mjPI / nedge); + uservert[3 * i + 2] = 1; + } + } else { + uservert[3 * nedge + 2] = 1; + } + + mjs_setFloat(spec.uservert, uservert.data(), n); } + + // compute vertex normals void mjCMesh::MakeNormal() { // only if normal data is missing @@ -2435,8 +2484,7 @@ void MeshPolygon::InsertFace(int v1, int v2, int v3) { -// return the traverse vertices of the polygon; there may be multiple paths if the polygon is -// not connected +// return the transverse vertices of the polygon, multiple paths possible if not connected std::vector > MeshPolygon::Paths() const { std::vector > paths; // shortcut if polygon is just a triangular face @@ -2550,6 +2598,7 @@ void mjCMesh::MakePolygons() { } + //------------------ class mjCSkin implementation -------------------------------------------------- // constructor diff --git a/src/user/user_objects.h b/src/user/user_objects.h index ed7d9be0..f2fc5661 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1053,7 +1053,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh { // make a mesh of a predefined shape void MakeWedge(int resolution[2], double fov[2], double gamma); void MakeRect(int resolution[2]); - void MakePrism(int nedge); + void MakeCone(int nedge, double radius); // accessors const mjsPlugin& Plugin() const { return plugin; } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 253b0d86..2f3f51c2 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -827,12 +827,11 @@ const mjMap meshinertia_map[4] = { // mesh builtin type -const int meshbuiltin_sz = 8; +const int meshbuiltin_sz = 7; const mjMap meshbuiltin_map[meshbuiltin_sz] = { {"none", mjMESH_BUILTIN_NONE}, {"sphere", mjMESH_BUILTIN_SPHERE}, {"hemisphere", mjMESH_BUILTIN_HEMISPHERE}, - {"prism", mjMESH_BUILTIN_PRISM}, {"cone", mjMESH_BUILTIN_CONE}, {"torus", mjMESH_BUILTIN_TORUS}, {"wedge", mjMESH_BUILTIN_WEDGE}, diff --git a/test/user/testdata/makemesh.xml b/test/user/testdata/makemesh.xml index 00c977e1..91955db9 100644 --- a/test/user/testdata/makemesh.xml +++ b/test/user/testdata/makemesh.xml @@ -1,13 +1,18 @@ - + - - - + + + + + - - - + + + + + + diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 858c305a..787ae3cd 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -2044,10 +2044,10 @@ TEST_F(XMLReaderTest, UnsupportedMesh) { static constexpr char xml[] = R"( - + - + )"; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 7b98e53c..c8fca9f7 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -501,11 +501,10 @@ public enum mjtMeshBuiltin : int{ mjMESH_BUILTIN_NONE = 0, mjMESH_BUILTIN_SPHERE = 1, mjMESH_BUILTIN_HEMISPHERE = 2, - mjMESH_BUILTIN_PRISM = 3, - mjMESH_BUILTIN_CONE = 4, - mjMESH_BUILTIN_TORUS = 5, - mjMESH_BUILTIN_WEDGE = 6, - mjMESH_BUILTIN_PLATE = 7, + mjMESH_BUILTIN_CONE = 3, + mjMESH_BUILTIN_TORUS = 4, + mjMESH_BUILTIN_WEDGE = 5, + mjMESH_BUILTIN_PLATE = 6, } public enum mjtBuiltin : int{ mjBUILTIN_NONE = 0,