Add built-in supertoroid mesh.

PiperOrigin-RevId: 791180080
Change-Id: I3a70f50ff4a52a1bd4b88fc30334a574895145b4

doc/XMLreference.rst

@@ -1348,11 +1348,15 @@ The full list of processing steps applied by the compiler to each mesh is as fol
       **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.
+   :at-val:`supertorus` (resolution, radius, s, t)
+      A generalization of a torus with major radius of 1 and given minor radius. If the **s** and **t** parameters are
+      both 1, the shape is a torus. See `here <https://en.wikipedia.org/wiki/Supertoroid>`__ for details regarding
+      the definition of supertori.
 
-      **radius**: real in (0, 1]: The minor radius of the torus.
-      |br| **resolution** integer >= 4: The discretization of both major and minor radii.
+      **resolution** integer >= 4: The discretization of both major and minor radii.
+      |br| **radius**: real in (0, 1]: The minor radius of the torus.
+      |br| **s**: real > 0: The "squareness" of major sections.
+      |br| **t**: real > 0: The "squareness" of minor sections.
 
    :at-val:`wedge` (res_phi, res_theta, fov_phi, fov_theta, gamma)
       A slice of a unit spherical shell in spherical coordinates.

doc/includes/references.h

@@ -1789,7 +1789,7 @@ typedef enum mjtMeshBuiltin_ {      // type of built-in procedural mesh
   mjMESH_BUILTIN_SPHERE,            // sphere
   mjMESH_BUILTIN_HEMISPHERE,        // hemisphere
   mjMESH_BUILTIN_CONE,              // cone
-  mjMESH_BUILTIN_TORUS,             // torus
+  mjMESH_BUILTIN_SUPERTORUS,        // supertorus
   mjMESH_BUILTIN_WEDGE,             // wedge
   mjMESH_BUILTIN_PLATE,             // plate
 } mjtMeshBuiltin;

include/mujoco/mjspec.h

@@ -75,7 +75,7 @@ typedef enum mjtMeshBuiltin_ {      // type of built-in procedural mesh
   mjMESH_BUILTIN_SPHERE,            // sphere
   mjMESH_BUILTIN_HEMISPHERE,        // hemisphere
   mjMESH_BUILTIN_CONE,              // cone
-  mjMESH_BUILTIN_TORUS,             // torus
+  mjMESH_BUILTIN_SUPERTORUS,        // supertorus
   mjMESH_BUILTIN_WEDGE,             // wedge
   mjMESH_BUILTIN_PLATE,             // plate
 } mjtMeshBuiltin;

python/mujoco/introspect/enums.py

@@ -801,7 +801,7 @@ ENUMS: Mapping[str, EnumDecl] = dict([
              ('mjMESH_BUILTIN_SPHERE', 1),
              ('mjMESH_BUILTIN_HEMISPHERE', 2),
              ('mjMESH_BUILTIN_CONE', 3),
-             ('mjMESH_BUILTIN_TORUS', 4),
+             ('mjMESH_BUILTIN_SUPERTORUS', 4),
              ('mjMESH_BUILTIN_WEDGE', 5),
              ('mjMESH_BUILTIN_PLATE', 6),
          ]),

python/mujoco/specs.cc

@@ -1041,6 +1041,16 @@ PYBIND11_MODULE(_specs, m) {
         }
       },
       py::arg("nedge"), py::arg("radius"));
+  mjsMesh.def(
+      "make_supertorus",
+      [](raw::MjsMesh* self, int resolution, double radius, double s,
+         double t) {
+        double params[4] = {static_cast<double>(resolution), radius, s, t};
+        if (mjs_makeMesh(self, mjMESH_BUILTIN_SUPERTORUS, params, 4)) {
+          throw pybind11::value_error(mjs_getError(mjs_getSpec(self->element)));
+        }
+      },
+      py::arg("resolution"), py::arg("radius"), py::arg("s"), py::arg("t"));
   mjsMesh.def(
       "make_plate",
       [](raw::MjsMesh* self, std::array<int, 2>& resolution) {

python/mujoco/specs_test.py

@@ -502,13 +502,17 @@ class SpecsTest(absltest.TestCase):
     mesh = spec.add_mesh(name='sphere')
     mesh.make_sphere(subdivision=2)
 
+    mesh = spec.add_mesh(name='supertoroid')
+    mesh.make_supertorus(resolution=10, radius=0.5, s=1, t=1)
+
     model = spec.compile()
-    self.assertEqual(model.nmesh, 5)
+    self.assertEqual(model.nmesh, 6)
     self.assertEqual(model.mesh_vertnum[0], 25 * 25)
     self.assertEqual(model.mesh_vertnum[1], 10)
     self.assertEqual(model.mesh_vertnum[2], 7)
     self.assertEqual(model.mesh_vertnum[3], 2 * (4 + 1) * (4 + 2) + 2)
     self.assertEqual(model.mesh_vertnum[4], 2 + 10 * 4**2)
+    self.assertEqual(model.mesh_vertnum[5], 100)
 
   def test_compile_errors_with_line_info(self):
     spec = mujoco.MjSpec()

src/user/user_api.cc

@@ -542,6 +542,35 @@ int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int npar
       return 0;
     }
 
+    case mjMESH_BUILTIN_SUPERTORUS: {
+      if (nparams != 4) {
+        m->SetError(mjCError(0, "Supertorus mesh type requires 4 parameters"));
+        return -1;
+      }
+      int res = static_cast<int>(params[0]);
+      if (res < 3) {
+        m->SetError(mjCError(0, "Supertorus resolution must be greater than 3"));
+        return -1;
+      }
+      double radius = params[1];
+      if (radius <= 0 || radius > 1) {
+        m->SetError(mjCError(0, "Supertorus radius must be in (0, 1]"));
+        return -1;
+      }
+      double s = params[2];
+      if (s <= 0) {
+        m->SetError(mjCError(0, "Supertorus 's' must be greater than 0"));
+        return -1;
+      }
+      double t = params[3];
+      if (t <= 0) {
+        m->SetError(mjCError(0, "Supertorus 't' must be greater than 0"));
+        return -1;
+      }
+      meshC->MakeSupertorus(res, radius, s, t);
+      return 0;
+    }
+
     case mjMESH_BUILTIN_WEDGE: {
       if (nparams != 5) {
         m->SetError(mjCError(0, "Wedge builtin mesh types require 5 parameters"));

src/user/user_mesh.cc

@@ -76,6 +76,9 @@ namespace {
   using mujoco::user::FilePath;
   using std::max;
   using std::min;
+  using std::sin;
+  using std::cos;
+  using std::pow;
 
   // Parametrized linear/quintic interpolated nonlinearity.
   double Fovea(double x, double gamma) {
@@ -84,7 +87,7 @@ namespace {
 
     // Foveal deformation.
     double g = mjMAX(0, mjMIN(1, gamma));
-    return g * std::pow(x, 5) + (1 - g) * x;
+    return g * pow(x, 5) + (1 - g) * x;
   }
 
   // Evenly spaced numbers over a specified interval.
@@ -120,18 +123,16 @@ namespace {
   // Transform spherical (azimuth, elevation, radius) to Cartesian (x,y,z).
   void SphericalToCartesian(const double aer[3], float xyz[3]) {
     double a = aer[0], e = aer[1], r = aer[2];
-    xyz[0] = r * std::cos(e) * std::sin(a);
-    xyz[1] = r * std::sin(e);
-    xyz[2] = -r * std::cos(e) * std::cos(a);
+    xyz[0] = r * cos(e) * sin(a);
+    xyz[1] = r * sin(e);
+    xyz[2] = -r * cos(e) * cos(a);
   }
 
   // Tangent frame in Cartesian coordinates.
   void TangentFrame(const double aer[3], float mat[9]) {
     double a = aer[0], e = aer[1], r = aer[2];
-    double ta[3] = {r * std::cos(e) * std::cos(a), 0,
-                    r * std::cos(e) * std::sin(a)};
-    double te[3] = {-r * std::sin(e) * std::sin(a), r * std::cos(e),
-                    r * std::sin(e) * std::cos(a)};
+    double ta[3] = {r * cos(e) * cos(a), 0, r * cos(e) * sin(a)};
+    double te[3] = {-r * sin(e) * sin(a), r * cos(e), r * sin(e) * cos(a)};
     double n[3];
     mjuu_normvec(ta, 3);
     mjuu_normvec(te, 3);
@@ -140,6 +141,14 @@ namespace {
     mjuu_crossvec(n, te, ta);
     mjuu_copyvec(mat, n, 3);
   }
+
+  // parametric superellipsoid/supertoroid helper functions
+  double aux_c(double omega, double m) {
+    return std::copysign(pow(std::abs(cos(omega)), m), cos(omega));
+  }
+  double aux_s(double omega, double m) {
+    return std::copysign(pow(std::abs(sin(omega)), m), sin(omega));
+  }
 }  // namespace
 
 // compute triangle area, surface normal, center
@@ -2283,6 +2292,112 @@ void mjCMesh::MakeSphere(int subdiv, bool make_faces) {
 
 
 
+// make a mesh of a torus (subsumed by supertorus, kept for reference only)
+void mjCMesh::MakeTorus(int res, double radius) {
+  // allocate vertices and faces
+  int nvert = res * res;
+  int nface = res * res * 2;
+  std::vector<float> vert(3 * nvert);
+  std::vector<int> face(3 * nface);
+
+  // generate vertices
+  for (int i = 0; i < res; ++i) {
+    for (int j = 0; j < res; ++j) {
+      double u = 2 * mjPI * i / res;
+      double v = 2 * mjPI * j / res;
+      int vidx = i * res + j;
+      vert[3 * vidx + 0] = (1 + radius * cos(v)) * cos(u);
+      vert[3 * vidx + 1] = (1 + radius * cos(v)) * sin(u);
+      vert[3 * vidx + 2] = radius * sin(v);
+    }
+  }
+
+  // generate faces
+  int fidx = 0;
+  for (int i = 0; i < res; ++i) {
+    for (int j = 0; j < res; ++j) {
+      int i_next = (i + 1) % res;
+      int j_next = (j + 1) % res;
+
+      int v1 = i * res + j;
+      int v2 = i_next * res + j;
+      int v3 = i_next * res + j_next;
+      int v4 = i * res + j_next;
+
+      // first triangle
+      face[3 * fidx + 0] = v1;
+      face[3 * fidx + 1] = v2;
+      face[3 * fidx + 2] = v4;
+      fidx++;
+
+      // second triangle
+      face[3 * fidx + 0] = v2;
+      face[3 * fidx + 1] = v3;
+      face[3 * fidx + 2] = v4;
+      fidx++;
+    }
+  }
+
+  // save vertices and faces
+  mjs_setFloat(spec.uservert, vert.data(), vert.size());
+  mjs_setInt(spec.userface, face.data(), face.size());
+}
+
+
+
+// make a mesh of a supertoroid, see https://en.wikipedia.org/wiki/Supertoroid
+void mjCMesh::MakeSupertorus(int res, double radius, double s, double t) {
+  // allocate vertices and faces
+  int nvert = res * res;
+  int nface = res * res * 2;
+  std::vector<float> vert(3 * nvert);
+  std::vector<int> face(3 * nface);
+
+  // generate vertices
+  for (int i = 0; i < res; ++i) {
+    for (int j = 0; j < res; ++j) {
+      double u = 2 * mjPI * i / res;
+      double v = 2 * mjPI * j / res;
+      int vidx = i * res + j;
+      vert[3 * vidx + 0] = (1 + radius * aux_c(v, s)) * aux_c(u, t);
+      vert[3 * vidx + 1] = (1 + radius * aux_c(v, s)) * aux_s(u, t);
+      vert[3 * vidx + 2] = radius * aux_s(v, s);
+    }
+  }
+
+  // generate faces
+  int fidx = 0;
+  for (int i = 0; i < res; ++i) {
+    for (int j = 0; j < res; ++j) {
+      int i_next = (i + 1) % res;
+      int j_next = (j + 1) % res;
+
+      int v1 = i * res + j;
+      int v2 = i_next * res + j;
+      int v3 = i_next * res + j_next;
+      int v4 = i * res + j_next;
+
+      // first triangle
+      face[3 * fidx + 0] = v1;
+      face[3 * fidx + 1] = v2;
+      face[3 * fidx + 2] = v4;
+      fidx++;
+
+      // second triangle
+      face[3 * fidx + 0] = v2;
+      face[3 * fidx + 1] = v3;
+      face[3 * fidx + 2] = v4;
+      fidx++;
+    }
+  }
+
+  // save vertices and faces
+  mjs_setFloat(spec.uservert, vert.data(), vert.size());
+  mjs_setInt(spec.userface, face.data(), face.size());
+}
+
+
+
 // make a mesh of a spherical wedge
 void mjCMesh::MakeWedge(int resolution[2], double fov[2], double gamma) {
   std::vector<double> x_edges(resolution[0] + 1, 0);
@@ -2361,16 +2476,16 @@ void mjCMesh::MakeCone(int nedge, double radius) {
 
   // bottom face
   for (int i = 0; i < nedge; i++) {
-    uservert[3 * i + 0] = std::cos(2 * i * mjPI / nedge);
-    uservert[3 * i + 1] = std::sin(2 * i * mjPI / nedge);
+    uservert[3 * i + 0] = cos(2 * i * mjPI / nedge);
+    uservert[3 * i + 1] = sin(2 * i * mjPI / nedge);
     uservert[3 * i + 2] = -1;
   }
 
   // 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 + 0] = radius * cos(2 * i * mjPI / nedge);
+      uservert[3 * i + 1] = radius * sin(2 * i * mjPI / nedge);
       uservert[3 * i + 2] = 1;
     }
   } else {
@@ -3660,7 +3775,7 @@ void inline ComputeLinearStiffness(std::vector<double>& K,
                                    double E, double nu) {
   // only linear elements are supported for now
   int order = 2;
-  int n = std::pow(order, 3);
+  int n = pow(order, 3);
   int ndof = 3*n;
 
   // compute quadrature points

src/user/user_objects.h

@@ -1053,6 +1053,8 @@ class mjCMesh: public mjCMesh_, private mjsMesh {
   // make a mesh of a predefined shape
   void MakeHemisphere(int res, bool make_faces, bool make_cap);
   void MakeSphere(int subdiv, bool make_faces);
+  void MakeTorus(int res, double radius);
+  void MakeSupertorus(int res, double radius, double s, double t);
   void MakeWedge(int resolution[2], double fov[2], double gamma);
   void MakeRect(int resolution[2]);
   void MakeCone(int nedge, double radius);

src/xml/xml_native_reader.cc

@@ -833,7 +833,7 @@ const mjMap meshbuiltin_map[meshbuiltin_sz] = {
   {"sphere", mjMESH_BUILTIN_SPHERE},
   {"hemisphere", mjMESH_BUILTIN_HEMISPHERE},
   {"cone", mjMESH_BUILTIN_CONE},
-  {"torus", mjMESH_BUILTIN_TORUS},
+  {"supertorus", mjMESH_BUILTIN_SUPERTORUS},
   {"wedge", mjMESH_BUILTIN_WEDGE},
   {"plate", mjMESH_BUILTIN_PLATE}
 };

test/user/testdata/makemesh.xml

@@ -2,6 +2,7 @@
   <asset>
     <mesh name="sphere" builtin="sphere" params="2" scale=".8 .8 .8"/>
     <mesh name="hemisphere" builtin="hemisphere" params="4" scale=".8 .8 .8"/>
+    <mesh name="toroid" builtin="supertorus" params="50 .5 .42 1.42" scale=".8 .8 .8"/>
     <mesh name="wedge" builtin="wedge" params="10 10 60 60 0"/>
     <mesh name="prism3" builtin="cone" params="3 1" scale=".3 .3 .3"/>
     <mesh name="prism5" builtin="cone" params="5 1" scale=".3 .3 .1"/>
@@ -13,6 +14,7 @@
     <light pos="0 0 10"/>
     <geom mesh="sphere" type="mesh" pos="-3 0 0"/>
     <geom mesh="hemisphere" type="mesh" pos="-1 0 0"/>
+    <geom mesh="toroid" type="mesh" pos="-1 0 2"/>
     <geom mesh="wedge" type="mesh" euler="90 -90 0"/>
     <geom mesh="prism5" type="mesh" pos="2 0 0" euler="90 0 0"/>
     <geom mesh="prism3" type="mesh" pos="3 0 0"/>

test/xml/xml_native_reader_test.cc

@@ -2040,24 +2040,6 @@ TEST_F(XMLReaderTest, ReadWedgeMesh) {
   mj_deleteModel(model);
 }
 
-TEST_F(XMLReaderTest, UnsupportedMesh) {
-  static constexpr char xml[] = R"(
-  <mujoco>
-    <asset>
-      <mesh name="torus" builtin="torus" params="25 25 180 90 0"/>
-    </asset>
-    <worldbody>
-      <geom type="mesh" mesh="torus"/>
-    </worldbody>
-  </mujoco>
-  )";
-  std::array<char, 1024> error;
-  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
-  ASSERT_THAT(model, IsNull());
-  EXPECT_THAT(error.data(), HasSubstr("Unsupported mesh type"));
-  mj_deleteModel(model);
-}
-
 TEST_F(XMLReaderTest, BuiltinAndFile) {
   static constexpr char xml[] = R"(
   <mujoco>

unity/Runtime/Bindings/MjBindings.cs

@@ -502,7 +502,7 @@ public enum mjtMeshBuiltin : int{
   mjMESH_BUILTIN_SPHERE = 1,
   mjMESH_BUILTIN_HEMISPHERE = 2,
   mjMESH_BUILTIN_CONE = 3,
-  mjMESH_BUILTIN_TORUS = 4,
+  mjMESH_BUILTIN_SUPERTORUS = 4,
   mjMESH_BUILTIN_WEDGE = 5,
   mjMESH_BUILTIN_PLATE = 6,
 }