chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
d168eb2d7de558ccc2fa3e25e305dd7a777243ca
Mujoco_WASM/test/user/user_mesh_test.cc
T
Alessio Quaglino 9c6a4f76eb Add 2D membrane elasticity for interpolated flex shell mode 
When elastic2d="stretch" is set on an interpolated flexcomp, treat the bounding box boundary as membrane elements rather than volumetric cells. This computes plane-stress stiffness over the boundary faces and updates the runtime force/derivative kernels accordingly.

Interior vertex tracking (moving vertices that follow the deforming shell) is not yet implemented so all mesh vertices need to be on the bounding box surface or the background grid should have no interior nodes (i.e. cellcount should be 1 on at least one axis).

PiperOrigin-RevId: 907654080
Change-Id: I51b90e2f6a1d1b036f9604e42de20e377dc5d3f9
2026-04-29 10:17:21 -07:00

1790 lines
57 KiB
C++
Raw Blame History

// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Tests for user/user_objects.cc.
#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <limits>
#include <memory>
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <absl/strings/str_format.h>
#include <absl/strings/str_replace.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "src/cc/array_safety.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
using MjCMeshTest = MujocoTest;
static const char* const kMeshPath =
"user/testdata/mesh.xml";
static const char* const kDuplicateVerticesPath =
"user/testdata/duplicate_vertices.xml";
static const char* const kCubePath =
"user/testdata/cube.xml";
static const char* const kCubeCompletePath =
"user/testdata/cube_complete.obj";
static const char* const kTorusPath =
"user/testdata/torus.xml";
static const char* const kTorusMaxhullVertPath =
"user/testdata/torus_maxhullvert.xml";
static const char* const kTorusDefaultMaxhullVertPath =
"user/testdata/torus_maxhullvert_default.xml";
static const char* const kCompareInertiaPath =
"user/testdata/inertia_compare.xml";
static const char* const kConvexInertiaPath =
"user/testdata/inertia_convex.xml";
static const char* const kConcaveInertiaPath =
"user/testdata/inertia_concave.xml";
static const char* const kShellInertiaPath =
"user/testdata/inertia_shell.xml";
static const char* const kTorusQuadsPath =
"user/testdata/torus_quads.xml";
static const char* const kTexturedTorusPath =
"user/testdata/textured_torus.xml";
static const char* const kDuplicateOBJPath =
"user/testdata/duplicate.xml";
static const char* const kMalformedFaceOBJPath =
"user/testdata/malformed_face.xml";
static const char* const kCubeSkinPath =
"user/testdata/cube_skin.xml";
static const char* const kNoDecoderForMeshErrorMsh =
"no decoder found for mesh";
using ::testing::ElementsAre;
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::NotNull;
static constexpr mjtNum kMaxAbsErr = std::numeric_limits<float>::epsilon();
// ------------- test invalid filenames ----------------------------------------
TEST_F(MjCMeshTest, UnknownMeshFormat) {
static constexpr char xml_format[] = R"(
<mujoco>
<asset>
<mesh name="m" file="%s"/>
</asset>
<worldbody>
<geom type="mesh" mesh="m"/>
</worldbody>
</mujoco>
)";
std::vector<std::string> invalid_names = {
"noextension",
"anobj",
"f",
"mesh.exe",
"file%s"
};
mjVFS vfs;
mj_defaultVFS(&vfs);
for (const auto& name : invalid_names) {
mj_addBufferVFS(&vfs, name.c_str(), nullptr, 0);
std::string xml = absl::StrFormat(xml_format, name);
std::array<char, 1024> error;
mjModel* model =
LoadModelFromString(xml.c_str(), error.data(), error.size(), &vfs);
ASSERT_THAT(model, testing::IsNull())
<< "Should fail to load a mesh named: " << name;
EXPECT_THAT(error.data(), HasSubstr(kNoDecoderForMeshErrorMsh));
EXPECT_THAT(error.data(), HasSubstr(name));
}
mj_deleteVFS(&vfs);
}
// -------------------- test OS filesystem fallback ----------------------------
TEST_F(MjCMeshTest, LoadMSHWithVFS) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh1" file="unknown_file.msh"/>
</asset>
<worldbody>
<geom type="mesh" mesh="mesh1"/>
</worldbody>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
// should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
TEST_F(MjCMeshTest, LoadOBJWithVFS) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh1" file="unknown_file.obj"/>
</asset>
<worldbody>
<geom type="mesh" mesh="mesh1"/>
</worldbody>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
// should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
TEST_F(MjCMeshTest, LoadSTLWithVFS) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh1" file="unknown_file.stl"/>
</asset>
<worldbody>
<geom type="mesh" mesh="mesh1"/>
</worldbody>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
// should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
// ------------- test content_type attributes ----------------------------------
TEST_F(MjCMeshTest, LoadMSHWithContentType) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh1" content_type="model/vnd.mujoco.msh" file="some_file"/>
</asset>
<worldbody>
<geom type="mesh" mesh="mesh1"/>
</worldbody>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
// should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
TEST_F(MjCMeshTest, LoadOBJWithContentType) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh1" content_type="model/obj" file="some_file"/>
</asset>
<worldbody>
<geom type="mesh" mesh="mesh1"/>
</worldbody>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
// should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
TEST_F(MjCMeshTest, LoadSTLWithContentType) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh1" content_type="model/stl" file="some_file"/>
</asset>
<worldbody>
<geom type="mesh" mesh="mesh1"/>
</worldbody>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
// should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh1" content_type="model/unknown" file="some_file"/>
</asset>
<worldbody>
<geom type="mesh" mesh="mesh1"/>
</worldbody>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
mjVFS vfs;
mj_defaultVFS(&vfs);
mj_addBufferVFS(&vfs, "some_file", nullptr, 0);
// should error with unknown file type
mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs);
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh));
mj_deleteVFS(&vfs);
}
TEST_F(MjCMeshTest, LoadMSHWithInvalidContentType) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh1" content_type="model" file="some_file"/>
</asset>
<worldbody>
<geom type="mesh" mesh="mesh1"/>
</worldbody>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
mjVFS vfs;
mj_defaultVFS(&vfs);
mj_addBufferVFS(&vfs, "some_file", nullptr, 0);
// should error with unknown file type
mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs);
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh));
mj_deleteVFS(&vfs);
}
TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh1" content_type="model/vnd.mujoco.msh;parameter=value" file="some_file"/>
</asset>
<worldbody>
<geom type="mesh" mesh="mesh1"/>
</worldbody>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
// should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get());
}
// ------------- test vertex deduplication (STL) ------------------------------
TEST_F(MjCMeshTest, DeduplicateSTLVertices) {
const std::string xml_path = GetTestDataFilePath(kDuplicateVerticesPath);
char error[1024];
size_t error_sz = 1024;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, error_sz);
ASSERT_THAT(model, NotNull()) << error;
ASSERT_EQ(model->nmeshvert, 4);
mj_deleteModel(model);
}
// -------------------- test Mesh loading (MSH) --------------------------------
TEST_F(MjCMeshTest, LoadMSH) {
const std::string xml_path = GetTestDataFilePath(kMeshPath);
char error[1024];
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
ASSERT_EQ(model->nmeshvert, 36);
mj_deleteModel(model);
}
// ------------- test OBJ loading ----------------------------------------------
using MjCMeshTest = MujocoTest;
TEST_F(MjCMeshTest, LoadCube) {
const std::string xml_path = GetTestDataFilePath(kCubePath);
mjModel* model = mj_loadXML(xml_path.c_str(), 0, nullptr, 0);
ASSERT_GT(model->ngeom, 0);
ASSERT_EQ(model->nmeshvert, 8);
ASSERT_EQ(model->nmeshface, 12);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, LoadTorus) {
const std::string xml_path = GetTestDataFilePath(kTorusPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_GT(model->ngeom, 0);
ASSERT_GT(model->nmeshvert, 0);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, LoadTorusQuads) {
const std::string xml_path = GetTestDataFilePath(kTorusQuadsPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_GT(model->ngeom, 0);
ASSERT_GT(model->nmeshvert, 0);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, LoadTexturedTorus) {
const std::string xml_path = GetTestDataFilePath(kTexturedTorusPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_GT(model->ngeom, 0);
ASSERT_GT(model->nmeshvert, 0);
ASSERT_GT(model->ntex, 0);
ASSERT_GT(model->ntexdata, 0);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, KeepDuplicateOBJVertices) {
const std::string xml_path = GetTestDataFilePath(kDuplicateOBJPath);
char error[1024];
size_t error_sz = 1024;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, error_sz);
ASSERT_EQ(model->nmeshvert, 16);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, SaveMeshOnce) {
const std::string xml_path = GetTestDataFilePath(kCubePath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
// Confirm the mesh file is loaded and stored in paths
EXPECT_EQ(
std::string(&model->paths[model->mesh_pathadr[0]]), "cube.obj");
std::string saved_xml = SaveAndReadXml(model);
EXPECT_THAT(saved_xml, Not(testing::HasSubstr("vertex")));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, TinyMeshLoads) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="tiny" vertex="0 0 0 1e-4 0 0 0 1e-4 0 0 0 1e-4"/>
</asset>
<worldbody>
<geom type="mesh" mesh="tiny"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
// ------------- test max hull vert -------------------------------------------
TEST_F(MjCMeshTest, MaxHullVert) {
const std::string xml_path = GetTestDataFilePath(kTorusMaxhullVertPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_GT(model->ngeom, 0);
ASSERT_EQ(model->mesh_graph[0], 4);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, MaxHullVertDefault) {
const std::string xml_path =
GetTestDataFilePath(kTorusDefaultMaxhullVertPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_GT(model->ngeom, 0);
ASSERT_EQ(model->mesh_graph[0], 64);
mj_deleteModel(model);
}
// ------------- test inline loading ------------------------------------------
TEST_F(MjCMeshTest, FaceNormalAutogenerated) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
normal="1 0 0 0 1 0 0 0 1 0.707 0 0.707"
face="0 2 1 0 3 2" />
</asset>
<worldbody>
<geom type="mesh" mesh="example_mesh"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
// ------------- test inertia -------------------------------------------------
TEST_F(MjCMeshTest, SmallInertiaLoads) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="tiny" vertex="0 0 0 1e-4 0 0 0 1e-4 0 0 0 1e-4"/>
</asset>
<worldbody>
<body>
<freejoint/>
<geom type="mesh" mesh="tiny"/>
<geom name="small" size=".001"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, TinyInertiaFails) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="tiny" vertex="0 0 0 1e-4 0 0 0 1e-4 0 0 0 1e-4"/>
</asset>
<worldbody>
<body name="tiny_body">
<freejoint/>
<geom type="mesh" mesh="tiny"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(
error.data(),
HasSubstr(
"mass and inertia of moving bodies must be larger than mjMINVAL"));
EXPECT_THAT(error.data(), HasSubstr("Element name 'tiny_body'"));
}
TEST_F(MjCMeshTest, FlippedFaceAllowedLegacyInertia) {
const std::string xml_path = GetTestDataFilePath(kMalformedFaceOBJPath);
char error[1024];
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_THAT(model->nmeshface, 4);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, MissingFaceAllowedConvexInertia) {
const std::string xml_path = GetTestDataFilePath(kCompareInertiaPath);
char error[1024];
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_THAT(model->nmeshface, 7);
EXPECT_NEAR(model->body_inertia[3], model->body_inertia[6], kMaxAbsErr);
EXPECT_NEAR(model->body_inertia[4], model->body_inertia[7], kMaxAbsErr);
EXPECT_NEAR(model->body_inertia[5], model->body_inertia[8], kMaxAbsErr);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, FlippedFaceFailsExactInertia) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh" inertia="exact"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="2 0 3 0 1 3 1 2 3 0 1 2" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr(
"Error: faces of mesh 'example_mesh' have inconsistent orientation. "
"Please check the faces containing the vertices 1 and 2."));
}
void CheckTetrahedronWasRescaled(mjModel* model) {
// The rotated and rescaled positions of the tetrahedron
// with vertices (0, 0, 0), (1, 0, 0), (0, 2, 0), (0, 0, 3)
// after mesh preprocessing is performed
std::vector<mjtNum> vert = {
-0.51610732078552246, -0.57402724027633667, -0.5283237099647522,
0.42337465286254883, -0.90627568960189819, -0.61189728975296021,
0.065528042614459991, 1.2306677103042603, -1.1645441055297852,
0.027204651385545731, 0.24963514506816864, 2.3047652244567871};
mjtNum tolerance = std::numeric_limits<float>::epsilon();
for (int i=0; i < 12; ++i) {
EXPECT_NEAR(model->mesh_vert[i], vert[i], tolerance);
}
}
TEST_F(MjCMeshTest, FlippedFaceAllowedWorld) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 2 0 0 0 3"
face="2 0 3 0 1 3 1 2 3 0 1 2" />
</asset>
<worldbody>
<geom type="mesh" mesh="example_mesh"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
CheckTetrahedronWasRescaled(model);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 2 0 0 0 3"
face="2 0 3 0 1 3 1 2 3 0 1 2" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh" mass="0"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
CheckTetrahedronWasRescaled(model);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, FlippedFaceAllowedInertial) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 2 0 0 0 3"
face="2 0 3 0 1 3 1 2 3 0 1 2" />
</asset>
<worldbody>
<body>
<inertial pos="0 0 0" mass="1"/>
<geom type="mesh" mesh="example_mesh"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
CheckTetrahedronWasRescaled(model);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 2 0 0 0 3 0 0 3"
face="2 0 3 0 1 3 1 2 3 0 2 1 0 3 4" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
CheckTetrahedronWasRescaled(model);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, AreaTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1e-8 0 0 0 1e-8 0 0 0 1e-8"
face="2 0 3 0 1 3 1 2 3 0 2 1" inertia="shell"/>
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small"));
}
TEST_F(MjCMeshTest, VolumeTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, VisualVolumeTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" mass="0"/>
</default>
</default>
<asset>
<mesh name="example_mesh"
vertex="0 -4e-16 0 1 0 4e-16 0 1 0 0 0 1"
face="0 2 1" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh" class="visual"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" mass="0"/>
</default>
</default>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 1 1 1e-6"
face="0 1 2 2 1 3" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh" class="visual"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_LE(mju_abs(model->geom_size[0]), 1);
EXPECT_LE(mju_abs(model->geom_size[1]), 1);
EXPECT_LE(mju_abs(model->geom_size[2]), 1);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 1 1 1e-6"
face="0 1 2 2 1 3" inertia="shell"/>
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="example_mesh"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_LE(mju_abs(model->geom_size[0]), 1);
EXPECT_LE(mju_abs(model->geom_size[1]), 1);
EXPECT_LE(mju_abs(model->geom_size[2]), 1);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="f" type="grid" count="3 3 1" spacing="1 1 1" dim="2" dof="2d">
<elasticity young="1" thickness="0" elastic2d="bend"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(),
HasSubstr("2d elasticity requires positive thickness"));
}
TEST_F(MjCMeshTest, InterpolatedFlexSupportsBendElasticityWithWarning) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="f" type="grid" count="3 3 2" spacing="1 1 1" dim="3" dof="trilinear">
<contact selfcollide="none"/>
<elasticity young="1" thickness="1" elastic2d="bend"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull()) << error.data();
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="f" type="grid" count="3 3 2" spacing="1 1 1" dim="3" dof="trilinear">
<contact selfcollide="none"/>
<elasticity young="1" thickness="1" elastic2d="both"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull()) << error.data();
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="f" type="grid" count="3 3 3" spacing="1 1 1" dim="3" dof="2d">
<elasticity young="1" thickness="1" elastic2d="bend"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex"));
}
TEST_F(MjCMeshTest, VolumeNegativeThrowsError) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
MESH_DEFINITIONS
</asset>
<worldbody>
<body>
GEOM_DEFINITIONS
</body>
</worldbody>
</mujoco>
)";
static constexpr char bad_mesh[] = R"(
<mesh name="bad_mesh%d" inertia="exact"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="3 0 2 0 3 1 1 3 2 0 1 2"/>\n"
)";
static constexpr char geom[] = R"(<geom type="mesh" mesh="bad_mesh%d"/>\n)";
for (int nmesh : {3, 16, 17, 50}) {
std::string mesh_definitions = "";
for (int i = 1; i < nmesh+1; i++) {
mesh_definitions += absl::StrFormat(bad_mesh, i);
}
std::string geom_definitions = "";
for (int i = 1; i < nmesh+1; i++) {
geom_definitions += absl::StrFormat(geom, i);
}
std::string xml_str = xml;
absl::StrReplaceAll({{"MESH_DEFINITIONS", mesh_definitions},
{"GEOM_DEFINITIONS", geom_definitions}}, &xml_str);
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml_str.c_str(),
error.data(), error.size());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("mesh volume is negative"));
}
}
TEST_F(MjCMeshTest, MeshIgnoresDefaultDensity) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<geom density="0" />
</default>
<asset>
<mesh name="a" vertex="0 0 0 1 0 0 0 1 0 0 0 1" scale="10 10 10" />
</asset>
<worldbody/>
</mujoco>)";
char error[1024];
mjSpec* spec = mj_parseXMLString(xml, 0, error, sizeof(error));
EXPECT_THAT(spec, NotNull()) << error;
mjModel* m1 = mj_compile(spec, nullptr);
EXPECT_THAT(m1, NotNull());
mj_deleteModel(m1);
mjModel* m2 = mj_compile(spec, nullptr);
EXPECT_THAT(m2, NotNull());
mj_deleteModel(m2);
mj_deleteSpec(spec);
}
// ------------- test concave and shell inertia --------------------------------
TEST_F(MjCMeshTest, ExactConcaveInertia) {
const std::string xml_path = GetTestDataFilePath(kConcaveInertiaPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
// analytic computation of 1x1x1 cube with a .8x.8x.9 hole
// see https://en.wikipedia.org/wiki/List_of_moments_of_inertia
mjtNum density = 2.;
mjtNum m_hole = .9 * .8 * .8 * density;
mjtNum m_cube = 1. * density;
mjtNum m_concave_cube = m_cube - m_hole;
mjtNum I_cube = m_cube/6.;
// due to the asymmetric hole, the com position has changed
// so we need to use https://en.wikipedia.org/wiki/Parallel_axis_theorem
mjtNum d_cube = .5 - model->body_ipos[5];
mjtNum d_hole = .55 - model->body_ipos[5];
mjtNum I1 = I_cube - m_hole*(.8*.8 + .8*.8)/12;
mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12
+ m_cube*d_cube*d_cube - m_hole*d_hole*d_hole;
EXPECT_NEAR(model->body_mass[1], m_concave_cube, kMaxAbsErr);
EXPECT_NEAR(model->body_mass[2], m_concave_cube, kMaxAbsErr);
EXPECT_NEAR(model->body_mass[3], m_concave_cube, kMaxAbsErr);
EXPECT_NEAR(model->body_mass[4], m_concave_cube, kMaxAbsErr);
for (int i = 3; i < 15; i += 3) {
EXPECT_NEAR(model->body_inertia[i], I1, kMaxAbsErr);
EXPECT_NEAR(model->body_inertia[i+1], I2, kMaxAbsErr);
EXPECT_NEAR(model->body_inertia[i+2], I2, kMaxAbsErr);
}
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, ExactConvexInertia) {
const std::string xml_path = GetTestDataFilePath(kConvexInertiaPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
// https://en.wikipedia.org/wiki/List_of_moments_of_inertia
mjtNum m_solid_cube = 1.;
mjtNum I_solid_cube = 1./6. * m_solid_cube;
EXPECT_LE(mju_abs(model->body_mass[1] - m_solid_cube), kMaxAbsErr);
EXPECT_LE(mju_abs(model->body_mass[2] - m_solid_cube), kMaxAbsErr);
for (int i = 3; i < 9; i++) {
EXPECT_LE(mju_abs(model->body_inertia[i] - I_solid_cube), kMaxAbsErr);
}
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, ExactShellInertia) {
const std::string xml_path = GetTestDataFilePath(kShellInertiaPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
// see https://en.wikipedia.org/wiki/List_of_moments_of_inertia
mjtNum m_hollow_cube = 6.;
mjtNum I_hollow_cube = 5./18. * m_hollow_cube;
EXPECT_LE(mju_abs(model->body_mass[1] - m_hollow_cube), kMaxAbsErr);
EXPECT_LE(mju_abs(model->body_inertia[3] - I_hollow_cube), kMaxAbsErr);
EXPECT_LE(mju_abs(model->body_inertia[4] - I_hollow_cube), kMaxAbsErr);
EXPECT_LE(mju_abs(model->body_inertia[5] - I_hollow_cube), kMaxAbsErr);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, MeshPosQuat) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="pyramid" vertex="0 0 0 1 0 0 0 1 0 0 0 1"/>
</asset>
<worldbody>
<geom type="mesh" name="geom1" mesh="pyramid"/>
<geom type="mesh" name="geom2" pos="1 2 3" quat="0.5 0.5 0.5 0.5" mesh="pyramid"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
// loading the mesh results in an offset of the geom's pos and quat due to the
// fact that the geom's center is not the volumetric center of the mesh. To
// recover the geom's originally specified pose, the offset used is stored in
// mesh_pos and mesh_quat. In order to recover the originally specified pose
// and orientation, first invert the specified mesh_pos and mesh_quat
mjtNum inverse_mesh_pos[3];
mjtNum inverse_mesh_quat[4];
mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
&model->mesh_pos[0], &model->mesh_quat[0]);
// apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
// quat. It should match the originally specified values
mjtNum recovered_pos[3];
mjtNum recovered_quat[4];
mju_mulPose(recovered_pos, recovered_quat,
&model->geom_pos[0], &model->geom_quat[0],
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[0], 1, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[1], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[2], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6));
// same test on the other geom
mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
&model->mesh_pos[0], &model->mesh_quat[0]);
mju_mulPose(recovered_pos, recovered_quat,
&model->geom_pos[3], &model->geom_quat[4],
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[0], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="pyramid" vertex="0 0 0 1 0 0 0 1 0 0 0 1" inertia="shell"/>
</asset>
<worldbody>
<geom type="mesh" name="geom1" mesh="pyramid"/>
<geom type="mesh" name="geom2" pos="1 2 3" quat="0.5 0.5 0.5 0.5" mesh="pyramid"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
// loading the mesh results in an offset of the geom's pos and quat due to the
// fact that the geom's center is not the volumetric center of the mesh. To
// recover the geom's originally specified pose, the offset used is stored in
// mesh_pos and mesh_quat. In order to recover the originally specified pose
// and orientation, first invert the specified mesh_pos and mesh_quat
mjtNum inverse_mesh_pos[3];
mjtNum inverse_mesh_quat[4];
mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
&model->mesh_pos[0], &model->mesh_quat[0]);
// apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
// quat. It should match the originally specified values
mjtNum recovered_pos[3];
mjtNum recovered_quat[4];
mju_mulPose(recovered_pos, recovered_quat,
&model->geom_pos[0], &model->geom_quat[0],
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[0], 1, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[1], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[2], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6));
// same test on the other geom
mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
&model->mesh_pos[0], &model->mesh_quat[0]);
mju_mulPose(recovered_pos, recovered_quat,
&model->geom_pos[3], &model->geom_quat[4],
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[0], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, MeshScale) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="pyramid" vertex="0 0 0 1 0 0 0 1 0 0 0 1"/>
<mesh name="pyramid_scaled" vertex="0 0 0 1 0 0 0 1 0 0 0 1" scale="0.9 1 -1"/>
</asset>
<worldbody>
<geom type="mesh" name="geom1" mesh="pyramid"/>
<geom type="mesh" name="geom2" mesh="pyramid_scaled"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1));
EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, NegativeScaleUserMeshCompiles) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh" scale="-1 1 1" inertia="exact"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1 0 3 2 1 3 0 1 2 3" />
</asset>
<worldbody>
<geom type="mesh" mesh="example_mesh"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) {
static constexpr char pos_xml[] = R"(
<mujoco>
<asset>
<mesh name="pos_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1 0 3 2 1 3 0 1 2 3" />
</asset>
<worldbody>
<geom type="mesh" mesh="pos_mesh"/>
</worldbody>
</mujoco>
)";
static constexpr char neg_xml[] = R"(
<mujoco>
<asset>
<mesh name="neg_mesh" scale="-1 1 1"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1 0 3 2 1 3 0 1 2 3" />
</asset>
<worldbody>
<geom type="mesh" mesh="neg_mesh"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* pos_model = LoadModelFromString(pos_xml, error, sizeof(error));
ASSERT_THAT(pos_model, NotNull()) << error;
mjModel* neg_model = LoadModelFromString(neg_xml, error, sizeof(error));
ASSERT_THAT(neg_model, NotNull()) << error;
ASSERT_EQ(pos_model->nmeshface, neg_model->nmeshface);
for (int i = 0; i < pos_model->nmeshface; i++) {
EXPECT_EQ(pos_model->mesh_face[3*i + 0], neg_model->mesh_face[3*i + 0]);
EXPECT_EQ(pos_model->mesh_face[3*i + 1], neg_model->mesh_face[3*i + 2]);
EXPECT_EQ(pos_model->mesh_face[3*i + 2], neg_model->mesh_face[3*i + 1]);
}
mj_deleteModel(pos_model);
mj_deleteModel(neg_model);
}
TEST_F(MjCMeshTest, ShellInertiaTest) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="pyramid" vertex="0 0 0 1 0 0 0 1 0 0 0 1" inertia="shell"/>
<mesh name="pyramid_scaled" vertex="0 0 0 1 0 0 0 1 0 0 0 1" scale="0.9 1 -1"/>
</asset>
<worldbody>
<geom type="mesh" name="geom1" mesh="pyramid"/>
<geom type="mesh" name="geom2" mesh="pyramid_scaled"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1));
EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1));
mj_deleteModel(model);
}
// ----------------------------- texcoord -------------------------------------
TEST_F(MjCMeshTest, CreateFaceTexCoord) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh" vertex="0 0 0 1 0 0 0 1 0 0 0 1"
texcoord="0 0 0 0 0 0 0 0"/>
</asset>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, UseFaceTexCoord) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh" vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1 0 1 3 2 0 3 1 2 3"
texcoord="0 0 .1 .1 .2 .2 .3 .3"/>
</asset>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, NotNull()) << error.data();
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 0]], .0);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 1]], .2);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 2]], .1);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 3]], .0);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 4]], .1);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 5]], .3);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 6]], .2);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 7]], .0);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 8]], .3);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[ 9]], .1);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[10]], .2);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2*model->mesh_facetexcoord[11]], .3);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, MissingTexCoord) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh" vertex="0 0 0 1 0 0 0 1 0 0 0 1"
texcoord="0 0"/>
</asset>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("texcoord must be 2*nv"));
}
// ----------------------------- qhull ----------------------------------------
TEST_F(MjCMeshTest, NaNConvexHullDisallowed) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh"vertex="nan 0 0 0 1 0 0 1 0 0 0 1"/>
</asset>
</mujoco>
)";
static char warning[1024];
warning[0] = '\0';
mju_user_warning = [](const char* msg) {
util::strcpy_arr(warning, msg);
};
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("vertex coordinate 0 is not finite"));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, InvalidIndexInFace) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="example_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
normal="1 0 0 0 1 0 0 0 1 0.707 0 0.707"
face="0 2 6 0 3 2" />
</asset>
<worldbody>
<geom type="mesh" mesh="example_mesh"/>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("in face 0, vertex index 6 does not exist"));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, QhullCache) {
static constexpr char xml1[] = R"(
<mujoco>
<asset>
<mesh name="box" file="cube_complete.obj"/>
</asset>
<worldbody>
<geom type="mesh" pos="0 0 2" mesh="box" contype="0" conaffinity="0"/>
<geom type="mesh" pos="0 0 0" mesh="box" contype="0" conaffinity="0"/>
</worldbody>
</mujoco>)";
static constexpr char xml2[] = R"(
<mujoco>
<asset>
<mesh name="box" file="cube_complete.obj"/>
</asset>
<worldbody>
<geom type="mesh" pos="0 0 2" mesh="box"/>
<geom type="mesh" pos="0 0 0" mesh="box"/>
</worldbody>
</mujoco>)";
mjVFS vfs;
mj_defaultVFS(&vfs);
mj_addFileVFS(&vfs, "", GetTestDataFilePath(kCubeCompletePath).c_str());
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml1, error.data(), error.size(), &vfs);
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
EXPECT_THAT(model->mesh_graphadr[0], -1);
mj_deleteModel(model);
model = LoadModelFromString(xml2, error.data(), error.size(), &vfs);
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
EXPECT_GT(model->mesh_graphadr[0], -1);
mj_deleteModel(model);
mj_deleteVFS(&vfs);
}
TEST_F(MjCMeshTest, LoadSkin) {
const std::string xml_path = GetTestDataFilePath(kCubeSkinPath);
std::array<char, 1024> error;
mjSpec* spec = mj_parseXML(xml_path.c_str(), 0, error.data(), error.size());
EXPECT_THAT(spec, NotNull()) << error.data();
mjModel* m1 = mj_compile(spec, 0);
EXPECT_THAT(m1, NotNull());
mj_deleteModel(m1);
mjModel* m2 = mj_compile(spec, 0);
EXPECT_THAT(m2, NotNull());
mj_deleteModel(m2);
mj_deleteSpec(spec);
}
// ------------- test octree ---------------------------------------------------
TEST_F(MjCMeshTest, Octree) {
const std::string xml_path = GetTestDataFilePath(kTorusPath);
std::array<char, 1024> error;
mjSpec* spec = mj_parseXML(xml_path.c_str(), 0, error.data(), error.size());
mjsGeom* geom = mjs_asGeom(mjs_firstElement(spec, mjOBJ_GEOM));
geom->type = mjGEOM_SDF;
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_GT(model->mesh_octnum[0], 0);
mj_deleteSpec(spec);
mj_deleteModel(model);
}
namespace {
bool AreAabbsAdjacent(const mjtNum* aabb1, const mjtNum* aabb2) {
const double kEps = 1e-6;
int touching_dims = 0;
int overlapping_dims = 0;
for (int dim = 0; dim < 3; ++dim) {
const mjtNum center1 = aabb1[dim];
const mjtNum half_size1 = aabb1[dim + 3];
const mjtNum center2 = aabb2[dim];
const mjtNum half_size2 = aabb2[dim + 3];
const mjtNum gap =
std::abs(center1 - center2) - (half_size1 + half_size2);
if (std::abs(gap) < kEps) {
touching_dims++;
} else if (gap < -kEps) {
overlapping_dims++;
}
}
return touching_dims == 1 && overlapping_dims == 2;
}
} // namespace
TEST_F(MjCMeshTest, OctreeIsBalanced) {
const std::string xml_path = GetTestDataFilePath(kTorusPath);
std::array<char, 1024> error;
mjSpec* spec = mj_parseXML(xml_path.c_str(), 0, error.data(), error.size());
mjsGeom* geom = mjs_asGeom(mjs_firstElement(spec, mjOBJ_GEOM));
geom->type = mjGEOM_SDF;
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_GT(model->mesh_octnum[0], 0);
const int octree_adr = model->mesh_octadr[0];
const int noct = model->mesh_octnum[0];
std::vector<int> leaves;
for (int i = 0; i < noct; ++i) {
bool is_leaf = true;
for (int j = 0; j < 8; ++j) {
if (model->oct_child[(octree_adr + i) * 8 + j] != -1) {
is_leaf = false;
break;
}
}
if (is_leaf) {
leaves.push_back(i);
}
}
int unbalanced_pairs = 0;
for (int i = 0; i < leaves.size(); ++i) {
for (int j = i + 1; j < leaves.size(); ++j) {
const int node1_idx = leaves[i];
const int node2_idx = leaves[j];
const mjtNum* aabb1 = &model->oct_aabb[(octree_adr + node1_idx) * 6];
const mjtNum* aabb2 = &model->oct_aabb[(octree_adr + node2_idx) * 6];
if (AreAabbsAdjacent(aabb1, aabb2)) {
const int level1 = model->oct_depth[octree_adr + node1_idx];
const int level2 = model->oct_depth[octree_adr + node2_idx];
if (std::abs(level1 - level2) > 1) {
if (unbalanced_pairs < 10) {
ADD_FAILURE()
<< "Nodes " << node1_idx << " (level " << level1 << ") and "
<< node2_idx << " (level " << level2 << ") are not balanced."
<< "\nAABB1: center=(" << aabb1[0] << ", " << aabb1[1] << ", "
<< aabb1[2] << "), half_size=(" << aabb1[3] << ", "
<< aabb1[4] << ", " << aabb1[5] << ")"
<< "\nAABB2: center=(" << aabb2[0] << ", " << aabb2[1] << ", "
<< aabb2[2] << "), half_size=(" << aabb2[3] << ", "
<< aabb2[4] << ", " << aabb2[5] << ")";
}
unbalanced_pairs++;
}
}
}
}
EXPECT_EQ(unbalanced_pairs, 0)
<< "Found " << unbalanced_pairs << " unbalanced adjacent leaf pairs.";
mj_deleteSpec(spec);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, OctreeHangingNodeInterpolation) {
const std::string xml_path = GetTestDataFilePath(kTorusPath);
std::array<char, 1024> error;
mjSpec* spec = mj_parseXML(xml_path.c_str(), 0, error.data(), error.size());
mjsGeom* geom = mjs_asGeom(mjs_firstElement(spec, mjOBJ_GEOM));
geom->type = mjGEOM_SDF;
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_GT(model->mesh_octnum[0], 0);
const mjtNum kEps = MjTol(1e-6, 1e-4);
const int octree_adr = model->mesh_octadr[0];
const int noct = model->mesh_octnum[0];
const mjtNum* sdf = model->oct_coeff + octree_adr * 8;
// find all leaves in the octree
std::vector<int> leaves;
for (int i = 0; i < noct; ++i) {
bool is_leaf = true;
for (int j = 0; j < 8; ++j) {
if (model->oct_child[(octree_adr + i) * 8 + j] != -1) {
is_leaf = false;
break;
}
}
if (is_leaf) {
leaves.push_back(i);
}
}
// do a n^2 check of all pairs of leaves in the octree
// for each pair, check if they are adjacent and if so, check that all hanging
// nodes within the octree can be interpolated from their parent nodes
int hanging_nodes_checked = 0;
int interpolation_failures = 0;
for (int i = 0; i < leaves.size(); ++i) {
for (int j = i + 1; j < leaves.size(); ++j) {
const int node1_idx = leaves[i];
const int node2_idx = leaves[j];
const mjtNum* aabb1 = &model->oct_aabb[(octree_adr + node1_idx) * 6];
const mjtNum* aabb2 = &model->oct_aabb[(octree_adr + node2_idx) * 6];
if (AreAabbsAdjacent(aabb1, aabb2)) {
const int level1 = model->oct_depth[octree_adr + node1_idx];
const int level2 = model->oct_depth[octree_adr + node2_idx];
if (level1 == level2) {
continue;
}
// decide which node is finer and which is coarser
const int finer_node_idx = (level1 > level2) ? node1_idx : node2_idx;
const int coarser_node_idx =
(level1 > level2) ? node2_idx : node1_idx;
const mjtNum* coarser_aabb =
&model->oct_aabb[(octree_adr + coarser_node_idx) * 6];
const mjtNum* finer_aabb =
&model->oct_aabb[(octree_adr + finer_node_idx) * 6];
mjtNum coarser_corners[8][3];
for (int c = 0; c < 8; ++c) {
int sx = (c & 1) ? 1 : -1;
int sy = (c & 2) ? 1 : -1;
int sz = (c & 4) ? 1 : -1;
coarser_corners[c][0] = coarser_aabb[0] + sx * coarser_aabb[3];
coarser_corners[c][1] = coarser_aabb[1] + sy * coarser_aabb[4];
coarser_corners[c][2] = coarser_aabb[2] + sz * coarser_aabb[5];
}
// for all vertices in the finer node, check if they are hanging and
// can be interpolated
for (int v_idx = 0; v_idx < 8; ++v_idx) {
mjtNum v_pos[3];
int sx = (v_idx & 1) ? 1 : -1;
int sy = (v_idx & 2) ? 1 : -1;
int sz = (v_idx & 4) ? 1 : -1;
v_pos[0] = finer_aabb[0] + sx * finer_aabb[3];
v_pos[1] = finer_aabb[1] + sy * finer_aabb[4];
v_pos[2] = finer_aabb[2] + sz * finer_aabb[5];
// skip finer vertices that are also coarse corners
bool is_coarse_corner = false;
for (int c = 0; c < 8; ++c) {
if (mju_dist3(v_pos, coarser_corners[c]) < 1e-6) {
is_coarse_corner = true;
break;
}
}
if (is_coarse_corner) {
continue;
}
// skip vertices that are not on the boundary
mjtNum p_local[3];
bool outside = false;
for (int d = 0; d < 3; ++d) {
p_local[d] = (v_pos[d] - coarser_aabb[d]) / coarser_aabb[d + 3];
if (std::abs(p_local[d]) > 1.0 + kEps) {
outside = true;
break;
}
}
if (outside) {
continue;
}
// count the number of dimensions that are on the boundary
int num_dim = 0;
for (int d = 0; d < 3; ++d) {
if (std::abs(p_local[d] - 1.0) < kEps) {
num_dim++;
} else if (std::abs(p_local[d] + 1.0) < kEps) {
num_dim++;
}
}
double interpolated_sdf = 0;
const mjtNum* coarser_sdf = sdf + coarser_node_idx * 8;
// for edge or face nodes, try to interpolate the hanging nodes
if (num_dim == 1 || num_dim == 2) {
for (int k = 0; k < 8; ++k) {
int sx = (k & 1) ? 1 : -1;
int sy = (k & 2) ? 1 : -1;
int sz = (k & 4) ? 1 : -1;
double weight = (1 + p_local[0] * sx) / 2.0 *
(1 + p_local[1] * sy) / 2.0 *
(1 + p_local[2] * sz) / 2.0;
if (weight > kEps && num_dim == 1) {
ASSERT_NEAR(weight, 0.25, kEps);
} else if (weight > kEps && num_dim == 2) {
ASSERT_NEAR(weight, 0.5, kEps);
}
interpolated_sdf += weight * coarser_sdf[k];
}
} else {
continue;
}
// if the values do not match, log an error
const mjtNum* finer_sdf = sdf + finer_node_idx * 8;
if (std::abs(finer_sdf[v_idx] - interpolated_sdf) > kEps) {
if (interpolation_failures < 10) {
EXPECT_NEAR(finer_sdf[v_idx], interpolated_sdf, kEps);
}
interpolation_failures++;
}
hanging_nodes_checked++;
}
}
}
}
EXPECT_GT(hanging_nodes_checked, 0);
EXPECT_EQ(interpolation_failures, 0)
<< "Found " << interpolation_failures
<< " hanging node interpolation failures.";
mj_deleteSpec(spec);
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, OctreeNotComputedForNonSDF) {
const std::string xml_path = GetTestDataFilePath(kTorusPath);
std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->noct, 0);
mj_deleteModel(model);
}
mjtNum CubeSDF(mjtNum p[3], mjtNum b[3]) {
mjtNum q[3] = {mju_abs(p[0]) - b[0],
mju_abs(p[1]) - b[1],
mju_abs(p[2]) - b[2]};
return mju_sqrt(std::pow(std::max(q[0], (mjtNum)0), 2) +
std::pow(std::max(q[1], (mjtNum)0), 2) +
std::pow(std::max(q[2], (mjtNum)0), 2)) +
std::min(std::max(q[0], std::max(q[1], q[2])), (mjtNum)0);
}
TEST_F(MjCMeshTest, OctreeCube) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh" vertex="-1 -1 -1 1 -1 -1 -1 1 -1 1 1 -1
-1 -1 1 1 -1 1 -1 1 1 1 1 1"/>
</asset>
<worldbody>
<geom mesh="mesh" type="sdf"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
EXPECT_EQ(m->noct, 63497);
mjData* d = mj_makeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
mjSDF sdf;
int instance = 0;
mjtGeom geomtype = mjGEOM_SDF;
const mjpPlugin* sdf_ptr = NULL;
sdf.id = &instance;
sdf.type = mjSDFTYPE_SINGLE;
sdf.plugin = &sdf_ptr;
sdf.geomtype = &geomtype;
mjtNum pnt[3][3] = {{1, 1, 1}, {.5, .5, .5}, {.5, 0, 0}};
mjtNum size[3] = {1, 1, 1};
for (int i = 0; i < 3; ++i) {
EXPECT_NEAR(mjc_distance(m, d, &sdf, pnt[i]), CubeSDF(pnt[i], size), 1e-1)
<< "i = " << i;
}
mj_deleteModel(m);
mj_deleteData(d);
}
TEST_F(MjCMeshTest, HemisphereSizes) {
static constexpr char xml[] = R"(
<mujoco model="makemesh">
<asset>
<mesh name="h0" builtin="hemisphere" params="0"/>
<mesh name="h1" builtin="hemisphere" params="1"/>
<mesh name="h2" builtin="hemisphere" params="2"/>
</asset>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->mesh_vertnum[0], 2 * (0 + 1) * (0 + 2) + 2);
EXPECT_EQ(model->mesh_vertnum[1], 2 * (1 + 1) * (1 + 2) + 2);
EXPECT_EQ(model->mesh_vertnum[2], 2 * (2 + 1) * (2 + 2) + 2);
EXPECT_EQ(model->mesh_facenum[0], 4 * (0 + 1) * (0 + 2));
EXPECT_EQ(model->mesh_facenum[1], 4 * (1 + 1) * (1 + 2));
EXPECT_EQ(model->mesh_facenum[2], 4 * (2 + 1) * (2 + 2));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, SphereSizes) {
static constexpr char xml[] = R"(
<mujoco model="makemesh">
<asset>
<mesh name="h0" builtin="sphere" params="0"/>
<mesh name="h1" builtin="sphere" params="1"/>
<mesh name="h2" builtin="sphere" params="2"/>
</asset>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->mesh_vertnum[0], 2 + 10 * std::pow(4, 0));
EXPECT_EQ(model->mesh_vertnum[1], 2 + 10 * std::pow(4, 1));
EXPECT_EQ(model->mesh_vertnum[2], 2 + 10 * std::pow(4, 2));
EXPECT_EQ(model->mesh_facenum[0], 20 * std::pow(4, 0));
EXPECT_EQ(model->mesh_facenum[1], 20 * std::pow(4, 1));
EXPECT_EQ(model->mesh_facenum[2], 20 * std::pow(4, 2));
mj_deleteModel(model);
}
TEST_F(MjCMeshTest, MeshMaterial) {
static constexpr char xml[] = R"(
<mujoco model="mesh_material">
<asset>
<mesh name="h0" builtin="sphere" params="0" material="m1"/>
<material name="m0" rgba="1 0 0 1"/>
<material name="m1" rgba="1 1 0 1"/>
</asset>
<worldbody>
<geom name="g0" type="mesh" mesh="h0"/>
<geom name="g1" type="mesh" mesh="h0" material="m0"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->geom_matid[0], 1);
EXPECT_EQ(model->geom_matid[1], 0);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
Reference in New Issue View Git Blame Copy Permalink