Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
314965a3cThis file is pinned to the selected release. Download the original to open it in your design tool.
// 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 <cstddef>
#include <cstring>
#include <memory>
#include <string>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
namespace mujoco {
namespace {
constexpr double kInertiaTol = MjTol(1e-6, 1e-6);
using std::string;
using ::testing::ElementsAre;
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::NotNull;
using ::testing::Pointwise;
// -------------------- test OS filesystem fallback ----------------------------
using VfsTest = MujocoTest;
TEST_F(VfsTest, HFieldPngWithVFS) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<hfield name="hfield" file="unknown_file.png" size="0.5 0.5 1 0.1"/>
</asset>
<worldbody>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05"/>
</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(VfsTest, HFieldCustomWithVFS) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<hfield name="hfield" file="unknown_file" size="0.5 0.5 1 0.1"/>
</asset>
<worldbody>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05"/>
</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(VfsTest, TexturePngWithVFS) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture name="texture" file="unknown_file.png" type="2d"/>
<material name="material" texture="texture"/>
</asset>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</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(VfsTest, TextureCustomWithVFS) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture name="texture" file="unknown_file" type="2d"/>
<material name="material" texture="texture"/>
</asset>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</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 attribute ------------------------
using ContentTypeTest = MujocoTest;
TEST_F(ContentTypeTest, HFieldPngWithContentType) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<hfield name="hfield" content_type="image/png"
file="some_file.png" size="0.5 0.5 1 0.1"/>
</asset>
<worldbody>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05"/>
</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 loading the file
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(ContentTypeTest, HFieldCustomWithContentType) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<hfield name="hfield" content_type="image/vnd.mujoco.hfield"
file="some_file" size="0.5 0.5 1 0.1"/>
</asset>
<worldbody>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05"/>
</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 loading the file
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(ContentTypeTest, HFieldWithContentTypeError) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<hfield name="hfield" content_type="image/jpeg"
file="some_file" size="0.5 0.5 1 0.1"/>
</asset>
<worldbody>
<geom type="hfield" hfield="hfield" pos="-.4 .6 .05"/>
</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 loading the file
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("unsupported content type: 'image/jpeg'"));
mj_deleteVFS(vfs.get());
}
TEST_F(ContentTypeTest, TexturePngWithContentType) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture name="texture" content_type="image/png" file="some_file" type="2d"/>
<material name="material" texture="texture"/>
</asset>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</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 loading the file
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(ContentTypeTest, TextureCustomWithContentType) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture name="texture" content_type="image/vnd.mujoco.texture"
file="some_file" type="2d"/>
<material name="material" texture="texture"/>
</asset>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</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 loading the file
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(ContentTypeTest, TextureWithContentTypeError) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture name="texture" content_type="image/jpeg"
file="some_file" type="2d"/>
<material name="material" texture="texture"/>
</asset>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</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 loading the file
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("unsupported content type: 'image/jpeg'"));
mj_deleteVFS(vfs.get());
}
TEST_F(ContentTypeTest, TextureLoadPng) {
static constexpr char filename[] = "tiny";
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture content_type="image/png" file="tiny" type="2d"/>
<material name="material" texture="tiny"/>
</asset>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</worldbody>
</mujoco>
)";
// tiny RGB 2 x 3 PNG file
static constexpr unsigned char tiny[] = {
0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, 0x0d,
0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02,
0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, 0x00, 0x00, 0x00,
0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, 0x78, 0xc1, 0xc0, 0xc0,
0xc0, 0xf0, 0xbf, 0xb8, 0xb8, 0x98, 0x81, 0xe1, 0x3f, 0xc3, 0xff, 0xff,
0xff, 0xc5, 0xc4, 0xc4, 0x00, 0x46, 0xd7, 0x07, 0x7f, 0xd2, 0x52, 0xa1,
0x41, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60,
0x82
};
size_t tiny_sz = sizeof(tiny);
char error[1024];
size_t error_sz = 1024;
// load VFS on the heap
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
mj_addBufferVFS(vfs.get(), filename, tiny, tiny_sz);
// loading the file should be successful
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, NotNull());
mj_deleteModel(model);
mj_deleteVFS(vfs.get());
}
// ------------------------ test keyframes -------------------------------------
using KeyframeTest = MujocoTest;
constexpr char kKeyframePath[] = "user/testdata/keyframe.xml";
TEST_F(KeyframeTest, CheckValues) {
const string xml_path = GetTestDataFilePath(kKeyframePath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
ASSERT_THAT(model, NotNull());
EXPECT_EQ(model->nkey, 7);
EXPECT_MJTNUM_EQ(model->key_time[0 * 1], 0.1);
EXPECT_MJTNUM_EQ(model->key_qpos[1 * model->nq], 0.2);
EXPECT_MJTNUM_EQ(model->key_qvel[2 * model->nv], 0.3);
EXPECT_MJTNUM_EQ(model->key_act[3 * model->na], 0.4);
EXPECT_THAT(AsVector(model->key_ctrl + 4 * model->nu, model->nu),
ElementsAre(0.5, 0.6));
EXPECT_THAT(AsVector(model->key_mpos + 3 * model->nmocap * 5, 3),
ElementsAre(.1, .2, .3));
EXPECT_THAT(AsVector(model->key_mquat + 4 * model->nmocap * 6, 4),
ElementsAre(.5, .5, .5, .5));
mj_deleteModel(model);
}
TEST_F(KeyframeTest, ResetDataKeyframe) {
const string xml_path = GetTestDataFilePath(kKeyframePath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
mj_resetDataKeyframe(model, data, 0);
EXPECT_MJTNUM_EQ(data->time, 0.1);
mj_resetDataKeyframe(model, data, 1);
EXPECT_MJTNUM_EQ(data->qpos[0], 0.2);
mj_resetDataKeyframe(model, data, 2);
EXPECT_MJTNUM_EQ(data->qvel[0], 0.3);
mj_resetDataKeyframe(model, data, 3);
EXPECT_MJTNUM_EQ(data->act[0], 0.4);
mj_resetDataKeyframe(model, data, 4);
EXPECT_MJTNUM_EQ(data->ctrl[0], 0.5);
EXPECT_MJTNUM_EQ(data->ctrl[1], 0.6);
mj_resetDataKeyframe(model, data, 5);
EXPECT_THAT(AsVector(data->mocap_pos, 3), ElementsAre(.1, .2, .3));
mj_resetDataKeyframe(model, data, 6);
EXPECT_THAT(AsVector(data->mocap_quat, 4), ElementsAre(.5, .5, .5, .5));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(KeyframeTest, ResetDataKeyframeAcceptsNegativeKeyframe) {
const string xml_path = GetTestDataFilePath(kKeyframePath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
data->time = data->qpos[0] = data->qvel[0] = data->act[0] = data->ctrl[0] =
data->mocap_pos[0] = data->mocap_quat[0] = 1337.0;
mj_resetDataKeyframe(model, data, -1);
EXPECT_EQ(data->time, 0.0);
EXPECT_EQ(data->qpos[0], 0.0);
EXPECT_EQ(data->qvel[0], 0.0);
EXPECT_EQ(data->act[0], 0.0);
EXPECT_EQ(data->ctrl[0], 0.0);
EXPECT_EQ(data->mocap_pos[0], 0.0);
EXPECT_EQ(data->mocap_quat[0], 1.0);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(KeyframeTest, BadSize) {
static constexpr char xml[] = R"(
<mujoco>
<keyframe>
<key qpos="1"/>
</keyframe>
</mujoco>
)";
char error[1024];
size_t error_sz = 1024;
mjModel* model = LoadModelFromString(xml, error, error_sz);
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error, HasSubstr("invalid qpos size, expected 0, got 1"));
}
// ------------- test relative frame sensor compilation-------------------------
using RelativeFrameSensorParsingTest = MujocoTest;
TEST_F(RelativeFrameSensorParsingTest, RefTypeNotRequired) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="sensorized"/>
</worldbody>
<sensor>
<framepos objtype="body" objname="sensorized"/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml, 0, 0);
ASSERT_THAT(model, NotNull());
mj_deleteModel(model);
}
TEST_F(RelativeFrameSensorParsingTest, ReferenceBodyFound) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="reference"/>
<body name="sensorized"/>
</worldbody>
<sensor>
<framepos objtype="xbody" objname="sensorized"
reftype="xbody" refname="reference"/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml, 0, 0);
ASSERT_THAT(model, NotNull());
ASSERT_EQ(model->sensor_reftype[0], mjOBJ_XBODY);
ASSERT_EQ(model->sensor_refid[0], 1);
mj_deleteModel(model);
}
TEST_F(RelativeFrameSensorParsingTest, MissingRefname) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="reference"/>
<body name="sensorized"/>
</worldbody>
<sensor>
<framepos objtype="body" objname="sensorized"
reftype="body" refname=""/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(error.data(), HasSubstr("missing name of reference frame"));
}
TEST_F(RelativeFrameSensorParsingTest, BadRefName) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="reference"/>
<body name="sensorized"/>
</worldbody>
<sensor>
<framepos objtype="body" objname="sensorized"
reftype="body" refname="wrong_name"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(error.data(),
HasSubstr("unrecognized name 'wrong_name' of object"));
EXPECT_THAT(error.data(), HasSubstr("line 8"));
}
TEST_F(RelativeFrameSensorParsingTest, BadRefType) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<light name="reference"/>
<body name="sensorized"/>
</worldbody>
<sensor>
<framepos objtype="body" objname="sensorized"
reftype="light" refname="reference"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(error.data(), HasSubstr("reference frame object must be"));
EXPECT_THAT(error.data(), HasSubstr("line 8"));
}
TEST_F(RelativeFrameSensorParsingTest, BadObjName) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site name="a"/>
</worldbody>
<sensor>
<framepos name="tom" objtype="site" objname="alessio" reftype="site" refname="a"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(),
HasSubstr("unrecognized name 'alessio' of sensorized object"));
EXPECT_THAT(error.data(), HasSubstr("name 'tom'"));
EXPECT_THAT(error.data(), HasSubstr("line 7"));
}
TEST_F(RelativeFrameSensorParsingTest, BadObjRefName) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site name="a"/>
</worldbody>
<sensor>
<framepos name="tom" objtype="site" objname="a" reftype="site" refname="alessio"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("unrecognized name 'alessio' of object"));
EXPECT_THAT(error.data(), HasSubstr("name 'tom'"));
EXPECT_THAT(error.data(), HasSubstr("line 7"));
}
// ------------- sensor compilation --------------------------------------------
using SensorTest = MujocoTest;
TEST_F(SensorTest, OjbtypeParsedButNotRequired) {
static constexpr char xml[] = R"(
<mujoco>
<sensor>
<user dim="3" needstage="vel" datatype="axis"/>
<user dim="2" needstage="pos" objtype="body" objname="world"/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml, 0, 0);
ASSERT_THAT(model, NotNull());
EXPECT_EQ(model->sensor_datatype[0], mjDATATYPE_AXIS);
EXPECT_EQ(model->sensor_objtype[0], mjOBJ_UNKNOWN);
EXPECT_EQ(model->sensor_dim[0], 3);
EXPECT_EQ(model->sensor_datatype[1], mjDATATYPE_REAL);
EXPECT_EQ(model->sensor_objtype[1], mjOBJ_BODY);
EXPECT_EQ(model->sensor_objid[1], 0);
mj_deleteModel(model);
}
TEST_F(SensorTest, NegativeIntervalError) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="j"/>
<geom size="1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="j" interval="-1"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("negative interval"));
}
TEST_F(SensorTest, PositivePhaseError) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="j"/>
<geom size="1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="j" interval="0.1 0.05"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("positive phase"));
}
TEST_F(SensorTest, DelayWithoutHistoryError) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="j"/>
<geom size="1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="j" delay="0.1"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("delay > 0 without a history buffer"));
}
// ------------- test capsule inertias -----------------------------------------
static const char* const kCapsuleInertiaPath =
"user/testdata/capsule_inertia.xml";
using MjCGeomTest = MujocoTest;
static constexpr int kSphereBodyId = 1, kCylinderBodyId = 2,
kCapsuleBodyId = 3, kCapsuleGeomId = 2;
TEST_F(MjCGeomTest, CapsuleMass) {
const string xml_path = GetTestDataFilePath(kCapsuleInertiaPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
// Mass of capsule should equal mass of cylinder + mass of sphere.
mjtNum sphere_cylinder_mass =
model->body_mass[kSphereBodyId] + model->body_mass[kCylinderBodyId];
mjtNum capsule_mass = model->body_mass[kCapsuleBodyId];
EXPECT_DOUBLE_EQ(sphere_cylinder_mass, capsule_mass);
mj_deleteModel(model);
}
TEST_F(MjCGeomTest, CapsuleInertiaZ) {
const string xml_path = GetTestDataFilePath(kCapsuleInertiaPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
// z-inertia of capsule should equal sphere + cylinder z-inertia.
mjtNum sphere_cylinder_z_inertia =
model->body_inertia[3 * kSphereBodyId + 2] +
model->body_inertia[3 * kCylinderBodyId + 2];
mjtNum capsule_z_inertia = model->body_inertia[3 * kCapsuleBodyId + 2];
EXPECT_DOUBLE_EQ(sphere_cylinder_z_inertia, capsule_z_inertia);
mj_deleteModel(model);
}
TEST_F(MjCGeomTest, CapsuleInertiaX) {
const string xml_path = GetTestDataFilePath(kCapsuleInertiaPath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
// The CoM of a solid hemisphere is 3/8*radius away from from the disk.
mjtNum hs_com = model->geom_size[3 * kCapsuleGeomId] * 3 / 8;
// The mass of the two hemispherical end-caps is just the mass of the sphere.
mjtNum sphere_mass = model->body_mass[1];
// x-inertia of capsule should equal sphere + cylinder x-inertias, with
// corrections from shifting the hemispheres using parallel axis theorem.
mjtNum sphere_cylinder_x_inertia = model->body_inertia[3 * kSphereBodyId] +
model->body_inertia[3 * kCylinderBodyId];
// Parallel axis-theorem #1: translate the hemispheres in to the origin.
mjtNum translate_in = hs_com;
sphere_cylinder_x_inertia -= sphere_mass * translate_in * translate_in;
// Parallel axis-theorem #2: translate the hemispheres out to the end caps.
mjtNum cylinder_half_length = model->geom_size[3 * kCapsuleGeomId + 1];
mjtNum translate_out = cylinder_half_length + hs_com;
sphere_cylinder_x_inertia += sphere_mass * translate_out * translate_out;
// Compare native capsule inertia and computed inertia.
mjtNum capsule_x_inertia = model->body_inertia[3 * kCapsuleBodyId];
EXPECT_DOUBLE_EQ(sphere_cylinder_x_inertia, capsule_x_inertia);
mj_deleteModel(model);
}
TEST_F(MjCGeomTest, ShellInertiaSphere) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision";
}
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom type="sphere"size="1.5" shellinertia="true"/>
</body>
<body>
<!-- mass is difference of body 4 and 3 masses -->
<geom type="sphere" size="1.5" mass="28.274333953857422" shellinertia="true"/>
</body>
<body>
<geom type="sphere" size="1.5" density="1e8"/>
</body>
<body>
<geom type="sphere" size="1.50000001" density="1e8"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// radius
mjtNum r = 1.5;
mjtNum r2 = r * r;
// body 1: shell inertia
mjtNum mass1 = 4 * mjPI * r2 * 1000; // surface area * surface density
EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol);
mjtNum I1 = 2 * mass1 * r2 / 3;
EXPECT_NEAR(m->body_inertia[3], I1, kInertiaTol);
EXPECT_NEAR(m->body_inertia[4], I1, kInertiaTol);
EXPECT_NEAR(m->body_inertia[5], I1, kInertiaTol);
// body 2: shell inertia, with specified mass
mjtNum I2 = 2 * m->body_mass[2] * r2 / 3;
EXPECT_NEAR(m->body_inertia[6], I2, kInertiaTol);
EXPECT_NEAR(m->body_inertia[7], I2, kInertiaTol);
EXPECT_NEAR(m->body_inertia[8], I2, kInertiaTol);
mjtNum mass3 = m->body_mass[3];
mjtNum mass4 = m->body_mass[4];
EXPECT_MJTNUM_EQ(mass4 - mass3, m->body_mass[2]);
// compute approximate shell inertia by subtracting inertias of massive bodies
// with small radius difference
mjtNum* inertia3 = m->body_inertia + 9;
mjtNum* inertia4 = m->body_inertia + 12;
mjtNum shell_inertia[3];
mju_sub3(shell_inertia, inertia4, inertia3);
EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol);
EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol);
EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol);
mj_deleteModel(m);
}
TEST_F(MjCGeomTest, ShellInertiaCapsule) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision";
}
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom type="capsule" size="0.1 0.25" shellinertia="true"/>
</body>
<!-- mass is difference of body 4 and 3 masses -->
<body>
<geom type="capsule" size="0.1 0.25" mass="4.3982325" shellinertia="true"/>
</body>
<body>
<geom type="capsule" size="0.1 0.25" density="1e8"/>
</body>
<body>
<geom type="capsule" size="0.1000001 0.25" density="1e8"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// dimensions
mjtNum r = 0.1;
mjtNum r2 = r * r;
mjtNum hh = 0.25;
mjtNum h = 2 * hh;
mjtNum h2 = h * h;
// hemisphere
mjtNum hs_com = r / 2; // height of hemisphere center of mass
mjtNum hs_pos = hh + hs_com; // distance from origin to hemisphere com
// surface area
double Asphere = 4 * mjPI * r2; // sphere
double Acylinder = 2 * mjPI * r * h; // cylinder
double Atotal = Asphere + Acylinder;
// body 1: shell inertia
mjtNum mass1 = Atotal * 1000; // surface area * surface density
EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol);
mjtNum mass1_sphere = mass1 * Asphere / Atotal;
mjtNum mass1_cylinder = mass1 - mass1_sphere;
double sphere1_inertia = 2 * mass1_sphere * r2 / 3;
mjtNum I1x = mass1_cylinder * (6 * r2 + h2) / 12 + sphere1_inertia +
mass1_sphere * (hs_pos * hs_pos - hs_com * hs_com);
mjtNum I1z = mass1_cylinder * r2 + sphere1_inertia;
EXPECT_NEAR(m->body_inertia[3], I1x, kInertiaTol);
EXPECT_NEAR(m->body_inertia[4], I1x, kInertiaTol);
EXPECT_NEAR(m->body_inertia[5], I1z, kInertiaTol);
// body 2: shell inertia, with specified mass
mjtNum mass2 = 4.3982325;
EXPECT_NEAR(m->body_mass[2], mass2, kInertiaTol);
EXPECT_FLOAT_EQ(m->body_mass[4] - m->body_mass[3], m->body_mass[2]);
mjtNum mass2_sphere = mass2 * Asphere / Atotal;
mjtNum mass2_cylinder = mass2 - mass2_sphere;
double sphere2_inertia = 2 * mass2_sphere * r2 / 3;
mjtNum I2x = mass2_cylinder * (6 * r2 + h2) / 12 + sphere2_inertia +
mass2_sphere * (hs_pos * hs_pos - hs_com * hs_com);
mjtNum I2z = mass2_cylinder * r2 + sphere2_inertia;
EXPECT_NEAR(m->body_inertia[6], I2x, kInertiaTol);
EXPECT_NEAR(m->body_inertia[7], I2x, kInertiaTol);
EXPECT_NEAR(m->body_inertia[8], I2z, kInertiaTol);
// compute approximate shell inertia by subtracting inertias of massive bodies
// with small radius difference
mjtNum* inertia3 = m->body_inertia + 9;
mjtNum* inertia4 = m->body_inertia + 12;
mjtNum shell_inertia[3];
mju_sub3(shell_inertia, inertia4, inertia3);
EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol);
EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol);
EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol);
mj_deleteModel(m);
}
TEST_F(MjCGeomTest, ShellInertiaCylinder) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision";
}
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom type="cylinder" size="0.1 0.25" shellinertia="true"/>
</body>
<!-- mass is difference of body 4 and 3 masses -->
<body>
<geom type="cylinder" size="0.1 0.25" mass="3.7699139" shellinertia="true"/>
</body>
<body>
<geom type="cylinder" size="0.1 0.25" density="1e8"/>
</body>
<body>
<geom type="cylinder" size="0.1000001 0.2500001" density="1e8"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// dimensions
mjtNum r = 0.1;
mjtNum hh = 0.25;
mjtNum r2 = r * r;
mjtNum h = 2 * hh;
mjtNum h2 = h * h;
// surface area
double Adisk = mjPI * r2; // disk
double Acylinder = 2 * mjPI * r * h; // cylinder
double Atotal = 2 * Adisk + Acylinder;
// body 1: shell inertia
mjtNum mass1 = Atotal * 1000; // surface area * surface density
EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol);
mjtNum mass1_disk = mass1 * Adisk / Atotal;
mjtNum mass1_cylinder = mass1 - 2 * mass1_disk;
mjtNum I1x = mass1_cylinder * (6 * r2 + h2) / 12 +
2 * (mass1_disk * r2 / 4 + mass1_disk * hh * hh);
mjtNum I1z = mass1_cylinder * r2 + mass1_disk * r2;
EXPECT_NEAR(m->body_inertia[3], I1x, kInertiaTol);
EXPECT_NEAR(m->body_inertia[4], I1x, kInertiaTol);
EXPECT_NEAR(m->body_inertia[5], I1z, kInertiaTol);
// body 2: shell inertia, with specified mass
mjtNum mass2 = 3.7699139;
EXPECT_NEAR(m->body_mass[2], mass2, kInertiaTol);
EXPECT_FLOAT_EQ(m->body_mass[4] - m->body_mass[3], m->body_mass[2]);
mjtNum mass2_disk = mass2 * Adisk / Atotal;
mjtNum mass2_cylinder = mass2 - 2 * mass2_disk;
mjtNum I2x = mass2_cylinder * (6 * r2 + h2) / 12 +
2 * (mass2_disk * r2 / 4 + mass2_disk * hh * hh);
mjtNum I2z = mass2_cylinder * r2 + mass2_disk * r2;
EXPECT_NEAR(m->body_inertia[6], I2x, kInertiaTol);
EXPECT_NEAR(m->body_inertia[7], I2x, kInertiaTol);
EXPECT_NEAR(m->body_inertia[8], I2z, kInertiaTol);
// compute approximate shell inertia by subtracting inertias of massive bodies
// with small radius difference
mjtNum* inertia3 = m->body_inertia + 9;
mjtNum* inertia4 = m->body_inertia + 12;
mjtNum shell_inertia[3];
mju_sub3(shell_inertia, inertia4, inertia3);
EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol);
EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol);
EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol);
mj_deleteModel(m);
}
TEST_F(MjCGeomTest, ShellInertiaEllipsoid) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision";
}
// test special case of ellipsoid with dimensions: a = b = c
// TODO(taylorhowell): add test for ellipsoid with dimensions: a != b != c
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom type="ellipsoid" size="0.1 0.1 0.1" shellinertia="true"/>
</body>
<body>
<!-- mass is difference of body 4 and 3 masses -->
<geom type="ellipsoid" size="0.1 0.1 0.1" mass="0.12566371" shellinertia="true"/>
</body>
<body>
<geom type="ellipsoid" size="0.1 0.1 0.1" density="1e8"/>
</body>
<body>
<geom type="ellipsoid" size="0.10000001 0.10000001 0.10000001" density="1e8"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// dimensions
mjtNum r = 0.1;
mjtNum r2 = r * r;
// body 1: shell inertia
mjtNum mass1 = 4 * mjPI * r2 * 1000; // surface area * surface density
EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol);
mjtNum I1 = 2 * mass1 * r2 / 3;
// note: increased tolerance, this is due to ellipsoid approximation
EXPECT_NEAR(m->body_inertia[3], I1, 10 * kInertiaTol);
EXPECT_NEAR(m->body_inertia[4], I1, 10 * kInertiaTol);
EXPECT_NEAR(m->body_inertia[5], I1, 10 * kInertiaTol);
// body 2: shell inertia, with specified mass
mjtNum mass2 = 0.12566371;
EXPECT_NEAR(m->body_mass[2], mass2, kInertiaTol);
EXPECT_FLOAT_EQ(m->body_mass[4] - m->body_mass[3], m->body_mass[2]);
mjtNum I2 = 2 * m->body_mass[2] * r2 / 3;
EXPECT_NEAR(m->body_inertia[6], I2, 10 * kInertiaTol);
EXPECT_NEAR(m->body_inertia[7], I2, 10 * kInertiaTol);
EXPECT_NEAR(m->body_inertia[8], I2, 10 * kInertiaTol);
// compute approximate shell inertia by subtracting inertias of massive bodies
// with small radius difference
mjtNum* inertia3 = m->body_inertia + 9;
mjtNum* inertia4 = m->body_inertia + 12;
mjtNum shell_inertia[3];
mju_sub3(shell_inertia, inertia4, inertia3);
EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], 10 * kInertiaTol);
EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], 10 * kInertiaTol);
EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], 10 * kInertiaTol);
mj_deleteModel(m);
}
TEST_F(MjCGeomTest, ShellInertiaBox) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision";
}
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom type="box" size="0.1 0.2 0.3" shellinertia="true"/>
</body>
<!-- mass is difference of body 4 and 3 masses -->
<body>
<geom type="box" size="0.1 0.2 0.3" mass="8.800005" shellinertia="true"/>
</body>
<body>
<geom type="box" size="0.1 0.2 0.3" density="1e8"/>
</body>
<body>
<geom type="box" size="0.1000001 0.2000001 0.3000001" density="1e8"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
// dimensions
mjtNum dx = 0.1;
mjtNum dy = 0.2;
mjtNum dz = 0.3;
// length
mjtNum lx = 2 * dx;
mjtNum ly = 2 * dy;
mjtNum lz = 2 * dz;
// surface area
double A0 = lx * ly;
double A1 = ly * lz;
double A2 = lz * lx;
double Atotal = 2 * (A0 + A1 + A2);
// body 1: shell inertia
mjtNum mass1 = Atotal * 1000; // surface area * surface density
EXPECT_NEAR(m->body_mass[1], mass1, kInertiaTol);
mjtNum mass1_0 = mass1 * A0 / Atotal;
mjtNum mass1_1 = mass1 * A1 / Atotal;
mjtNum mass1_2 = mass1 * A2 / Atotal;
mjtNum I1x = 2 * (mass1_0 * ly * ly / 12 + mass1_0 * dz * dz +
mass1_1 * (ly * ly + lz * lz) / 12 +
mass1_2 * lz * lz / 12 + mass1_2 * dy * dy);
mjtNum I1y =
2 * (mass1_0 * lx * lx / 12 + mass1_0 * dz * dz + mass1_1 * lz * lz / 12 +
mass1_1 * dx * dx + mass1_2 * (lx * lx + lz * lz) / 12);
mjtNum I1z =
2 * (mass1_0 * (lx * lx + ly * ly) / 12 + mass1_1 * ly * ly / 12 +
mass1_1 * dx * dx + mass1_2 * lx * lx / 12 + mass1_2 * dy * dy);
EXPECT_NEAR(m->body_inertia[3], I1x, kInertiaTol);
EXPECT_NEAR(m->body_inertia[4], I1y, kInertiaTol);
EXPECT_NEAR(m->body_inertia[5], I1z, kInertiaTol);
// body 2: shell inertia, with specified mass
mjtNum mass2 = 8.800005;
EXPECT_NEAR(m->body_mass[2], mass2, 1e-6);
EXPECT_FLOAT_EQ(m->body_mass[4] - m->body_mass[3], m->body_mass[2]);
mjtNum mass2_0 = mass2 * A0 / Atotal;
mjtNum mass2_1 = mass2 * A1 / Atotal;
mjtNum mass2_2 = mass2 * A2 / Atotal;
mjtNum I2x = 2 * (mass2_0 * ly * ly / 12 + mass2_0 * dz * dz +
mass2_1 * (ly * ly + lz * lz) / 12 +
mass2_2 * lz * lz / 12 + mass2_2 * dy * dy);
mjtNum I2y =
2 * (mass2_0 * lx * lx / 12 + mass2_0 * dz * dz + mass2_1 * lz * lz / 12 +
mass2_1 * dx * dx + mass2_2 * (lx * lx + lz * lz) / 12);
mjtNum I2z =
2 * (mass2_0 * (lx * lx + ly * ly) / 12 + mass2_1 * ly * ly / 12 +
mass2_1 * dx * dx + mass2_2 * lx * lx / 12 + mass2_2 * dy * dy);
EXPECT_NEAR(m->body_inertia[6], I2x, kInertiaTol);
EXPECT_NEAR(m->body_inertia[7], I2y, kInertiaTol);
EXPECT_NEAR(m->body_inertia[8], I2z, kInertiaTol);
// compute approximate shell inertia by subtracting inertias of massive bodies
// with small radius difference
mjtNum* inertia3 = m->body_inertia + 9;
mjtNum* inertia4 = m->body_inertia + 12;
mjtNum shell_inertia[3];
mju_sub3(shell_inertia, inertia4, inertia3);
EXPECT_NEAR(shell_inertia[0], m->body_inertia[6], kInertiaTol);
EXPECT_NEAR(shell_inertia[1], m->body_inertia[7], kInertiaTol);
EXPECT_NEAR(shell_inertia[2], m->body_inertia[8], kInertiaTol);
mj_deleteModel(m);
}
// ------------- test inertiagrouprange ----------------------------------------
TEST_F(MjCGeomTest, IgnoreGeomOutsideInertiagrouprange) {
static constexpr char xml[] = R"(
<mujoco>
<compiler inertiagrouprange="0 1"/>
<worldbody>
<body>
<geom size="1" group="3"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml, nullptr, 0);
EXPECT_THAT(m->body_mass[1], 0);
mj_deleteModel(m);
}
TEST_F(MjCGeomTest, IgnoreBadGeomOutsideInertiagrouprange) {
static constexpr char xml[] = R"(
<mujoco>
<compiler inertiagrouprange="0 1"/>
<asset>
<mesh name="malformed_mesh"
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="malformed_mesh" group="3"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml, nullptr, 0);
EXPECT_THAT(m->body_mass[1], 0);
mj_deleteModel(m);
}
// ------------- test invalid size values --------------------------------------
TEST_F(MjCGeomTest, NanSize) {
// even if the caller ignores warnings, models shouldn't compile with NaN
// geom sizes
mju_user_warning = nullptr;
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom type="box" size="1 1 nan" mass="1" />
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, testing::IsNull());
ASSERT_THAT(error.data(), HasSubstr("nan"));
EXPECT_THAT(error.data(), HasSubstr("line 5"));
}
TEST_F(MjCGeomTest, BadMeshZeroMassDensityDoesntError) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="bad_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1" inertia="shell"/>
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="bad_mesh" mass="0"/>
</body>
<body>
<geom type="mesh" mesh="bad_mesh" density="0"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->body_mass[1], 0);
EXPECT_EQ(model->body_mass[2], 0);
mj_deleteModel(model);
}
// ------------- test joints --------------------------------------------------
using MjCJointTest = MujocoTest;
TEST_F(MjCJointTest, AlignFree) {
const std::string xml_path =
GetTestDataFilePath("user/testdata/freejoint.xml");
std::array<char, 1024> err;
mjSpec* s = mj_parseXML(xml_path.c_str(), nullptr, err.data(), err.size());
ASSERT_THAT(s, NotNull()) << err.data();
s->compiler.alignfree = 1; // auto-aligned free joint
mjModel* m = mj_compile(s, nullptr);
ASSERT_THAT(m, NotNull());
// with alignfree the body has sameframe and simple and all dof are simple
EXPECT_EQ(m->body_sameframe[1], 1);
EXPECT_EQ(m->body_simple[1], 1);
EXPECT_EQ(m->dof_simplenum[0], 6);
// make unaligned model
s->compiler.alignfree = 0; // unaligned free joint
mjModel* m_u = mj_compile(s, nullptr);
ASSERT_THAT(m_u, NotNull());
// no sameframe or simple
EXPECT_EQ(m_u->body_sameframe[1], 0);
EXPECT_EQ(m_u->body_simple[1], 0);
// make datas for both models
mjData* d = mj_makeData(m);
mjData* d_u = mj_makeData(m_u);
// call mj_forward
mj_forward(m, d);
mj_forward(m_u, d_u);
// expect x-frames (sensors) to match to very high precision
double eps = MjTol(1e-10, 1e-6);
EXPECT_THAT(
AsVector(d->sensordata, m->nsensordata),
Pointwise(MjNear(eps, eps), AsVector(d_u->sensordata, m->nsensordata)));
// no frame sensors for lights, test separately
EXPECT_THAT(AsVector(d->light_xpos, 3),
Pointwise(MjNear(eps, eps), AsVector(d_u->light_xpos, 3)));
EXPECT_THAT(AsVector(d->light_xdir, 3),
Pointwise(MjNear(eps, eps), AsVector(d_u->light_xdir, 3)));
// reduce timestep to 0.1ms and use RK4, simulate for 1 second
m->opt.timestep = m_u->opt.timestep = 1e-4;
m->opt.integrator = m_u->opt.integrator = mjINT_RK4;
while (d->time < 1) {
mj_step(m, d);
mj_step(m_u, d_u);
}
// expect qpos to be significantly different, since the semantics are changed
mj_markStack(d);
int nq = m->nq;
mjtNum* dqpos = mj_stackAllocNum(d, nq);
mju_sub(dqpos, d->qpos, d_u->qpos, nq);
EXPECT_GT(mju_norm(dqpos, nq), 1.0);
mj_freeStack(d);
// expect x-frames to match to reasonable precision
eps = MjTol(1e-5, 1e-2);
EXPECT_THAT(
AsVector(d->sensordata, m->nsensordata),
Pointwise(MjNear(eps, eps), AsVector(d_u->sensordata, m->nsensordata)));
EXPECT_THAT(AsVector(d->light_xpos, 3),
Pointwise(MjNear(eps, eps), AsVector(d_u->light_xpos, 3)));
EXPECT_THAT(AsVector(d->light_xdir, 3),
Pointwise(MjNear(eps, eps), AsVector(d_u->light_xdir, 3)));
mj_deleteData(d_u);
mj_deleteData(d);
mj_deleteModel(m_u);
mj_deleteModel(m);
mj_deleteSpec(s);
}
// ------------- test height fields --------------------------------------------
using MjCHFieldTest = MujocoTest;
TEST_F(MjCHFieldTest, PngMap) {
const string xml_path = GetTestDataFilePath("user/testdata/hfield_png.xml");
std::array<char, 1024> error;
mjModel* model =
mj_loadXML(xml_path.c_str(), nullptr, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->nhfield, 1);
EXPECT_EQ(model->geom_type[0], mjGEOM_HFIELD);
EXPECT_GT(model->nhfielddata, 0);
float min_hfield = 1e7;
float max_hfield = -1e7;
for (int i = 0; i < model->nhfielddata; i++) {
float v = model->hfield_data[i];
min_hfield = std::min(min_hfield, v);
max_hfield = std::max(max_hfield, v);
}
EXPECT_EQ(min_hfield, 0) << "hfield should be normalised to [0, 1]";
EXPECT_EQ(max_hfield, 1) << "hfield should be normalised to [0, 1]";
mj_deleteModel(model);
}
// ------------- test textures -------------------------------------------------
using MjCTextureTest = MujocoTest;
TEST_F(MjCTextureTest, TexturesLoad) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture name="body" type="cube" builtin="flat" mark="cross" width="8"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/>
</asset>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mj_deleteModel(m);
}
// ------------- test quaternion normalization----------------------------------
using QuatNorm = MujocoTest;
TEST_F(QuatNorm, QuatNotNormalized) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site quat="1 2 2 4"/>
<camera quat="1 2 2 4"/>
<body quat="1 2 2 4">
<geom quat="1 2 2 4" size="1"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(AsVector(m->body_quat+4, 4), ElementsAre(1./5, 2./5, 2./5, 4./5));
EXPECT_THAT(AsVector(m->geom_quat, 4), ElementsAre(1./5, 2./5, 2./5, 4./5));
EXPECT_THAT(AsVector(m->site_quat, 4), ElementsAre(1./5, 2./5, 2./5, 4./5));
EXPECT_THAT(AsVector(m->cam_quat, 4), ElementsAre(1./5, 2./5, 2./5, 4./5));
mj_deleteModel(m);
}
// ------------- test camera specifications ------------------------------------
using CameraSpecTest = MujocoTest;
TEST_F(CameraSpecTest, FovyLimits) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<camera fovy="180"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("fovy too large"));
EXPECT_THAT(error.data(), HasSubstr("line 6"));
mj_deleteModel(m);
}
TEST_F(CameraSpecTest, DuplicatedFocalIgnorePixel) {
static constexpr char xml[] = R"(
<mujoco>
<visual>
<map znear="0.01"/>
</visual>
<worldbody>
<body>
<geom size="1"/>
<camera focal="8e-3 8e-3" focalpixel="100 100"
resolution="1920 1200" sensorsize="9.6e-3 6e-3"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, NotNull()) << error.data();
EXPECT_NEAR(m->cam_intrinsic[0], 5e-4, 1e-6); // focal length in meters (x)
EXPECT_NEAR(m->cam_intrinsic[1], 5e-4, 1e-6); // focal length in meters (y)
mj_deleteModel(m);
}
TEST_F(CameraSpecTest, FovyFromResolution) {
static constexpr char xml[] = R"(
<mujoco>
<visual>
<map znear="0.01"/>
</visual>
<worldbody>
<body>
<geom size="1"/>
<!-- 8mm focal length lenses -->
<camera focal="8e-3 8e-3" resolution="1920 1200" sensorsize="9.6e-3 6e-3"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, NotNull()) << error.data();
EXPECT_NEAR(m->cam_fovy[0], 41.112, 1e-3);
EXPECT_NEAR(m->cam_intrinsic[0], 8e-3, 1e-6); // focal length in meters (x)
EXPECT_NEAR(m->cam_intrinsic[1], 8e-3, 1e-6); // focal length in meters (y)
EXPECT_EQ(m->cam_intrinsic[2], 0); // principal point in meters (x)
EXPECT_EQ(m->cam_intrinsic[3], 0); // principal point in meters (y)
mj_deleteModel(m);
}
TEST_F(CameraSpecTest, FovyFromResolutionPixel) {
static constexpr char xml[] = R"(
<mujoco>
<visual>
<map znear="0.01"/>
</visual>
<worldbody>
<body>
<geom size="1"/>
<!-- 8mm focal length lenses -->
<camera focalpixel="1600 1600" resolution="1920 1200" sensorsize="9.6e-3 6e-3"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, NotNull()) << error.data();
EXPECT_NEAR(m->cam_fovy[0], 41.112, 1e-3);
EXPECT_NEAR(m->cam_intrinsic[0], 8e-3, 1e-6); // focal length in meters (x)
EXPECT_NEAR(m->cam_intrinsic[1], 8e-3, 1e-6); // focal length in meters (y)
EXPECT_EQ(m->cam_intrinsic[2], 0); // principal point in meters (x)
EXPECT_EQ(m->cam_intrinsic[3], 0); // principal point in meters (y)
mj_deleteModel(m);
}
TEST_F(CameraSpecTest, ParentTargetingNull) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom size="1"/>
<camera mode="targetbody" target="world"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, NotNull()) << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
// orientation can't be decided so we get an arbitrary but valid xmat
// (camera pointed towards the negative x-axis)
EXPECT_THAT(AsVector(data->cam_xmat, 9),
ElementsAre(0, 0, 1,
1, 0, 0,
0, 1, 0));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(CameraSpecTest, ParentTargeting) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom size="1"/>
<camera pos="1 1 0" mode="targetbody" target="world"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, NotNull()) << error.data();
mjData* data = mj_makeData(model);
mj_forward(model, data);
// expect negative z-axis to point from camera to world
EXPECT_FLOAT_EQ(data->cam_xmat[2], mju_sqrt(0.5));
EXPECT_FLOAT_EQ(data->cam_xmat[5], mju_sqrt(0.5));
EXPECT_FLOAT_EQ(data->cam_xmat[8], 0);
mj_deleteData(data);
mj_deleteModel(model);
}
// ------------- test actuator order -------------------------------------------
using ActuatorTest = MujocoTest;
TEST_F(ActuatorTest, ActuatorOrderDoesntMatter) {
static constexpr char xml1[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" dyntype="filter"/>
<general joint="hinge"/>
</actuator>
</mujoco>
)";
static constexpr char xml2[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<general joint="hinge"/>
<general joint="hinge" dyntype="filter"/>
</actuator>
</mujoco>
)";
mjModel* model1 = LoadModelFromString(xml1, nullptr, 0);
mjData* data1 = mj_makeData(model1);
mjModel* model2 = LoadModelFromString(xml2, nullptr, 0);
mjData* data2 = mj_makeData(model2);
// check activation indexing
EXPECT_EQ(model1->actuator_actadr[0], 0);
EXPECT_EQ(model1->actuator_actadr[1], -1);
EXPECT_EQ(model2->actuator_actadr[0], -1);
EXPECT_EQ(model2->actuator_actadr[1], 0);
// integrate both models, flipping the controls
while (data1->time < 1) {
data1->ctrl[0] = data1->time;
data1->ctrl[1] = -data1->time;
mj_step(model1, data1);
}
while (data2->time < 1) {
data2->ctrl[0] = -data2->time;
data2->ctrl[1] = data2->time;
mj_step(model2, data2);
}
// expect states to match exactly
EXPECT_EQ(data1->qpos[0], data2->qpos[0]);
EXPECT_EQ(data1->qvel[0], data2->qvel[0]);
EXPECT_EQ(data1->act[0], data2->act[0]);
mj_deleteData(data2);
mj_deleteModel(model2);
mj_deleteData(data1);
mj_deleteModel(model1);
}
// ------------- test inheritrange attribute ----------------------------------
using InheritrangeTest = MujocoTest;
TEST_F(InheritrangeTest, ErrorIfTargetMissingRange) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="jnt"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<position joint="jnt" inheritrange="1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("target 'jnt' has no range defined"));
}
TEST_F(InheritrangeTest, WorksForDegrees) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="jnt" range="90 180"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<position joint="jnt" inheritrange="1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_MJTNUM_EQ(model->actuator_ctrlrange[0], mjPI / 2);
EXPECT_MJTNUM_EQ(model->actuator_ctrlrange[1], mjPI);
mj_deleteModel(model);
}
// ------------- test actlimited and actrange fields ---------------------------
using ActRangeTest = MujocoTest;
TEST_F(ActRangeTest, ActRangeParsed) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<general dyntype="integrator" joint="hinge" actlimited="true" actrange="-1 1.5"/>
</actuator>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml, nullptr, 0);
EXPECT_EQ(m->actuator_actlimited[0], 1);
EXPECT_EQ(m->actuator_actrange[0], -1);
EXPECT_EQ(m->actuator_actrange[1], 1.5);
mj_deleteModel(m);
}
TEST_F(ActRangeTest, ActRangeBad) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<general dyntype="integrator" joint="hinge" actlimited="true" actrange="1 -1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid actrange"));
EXPECT_THAT(error.data(), HasSubstr("line 10"));
}
TEST_F(ActRangeTest, ActRangeUndefined) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<general dyntype="integrator" joint="hinge" actlimited="true"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid actrange"));
EXPECT_THAT(error.data(), HasSubstr("line 10"));
}
TEST_F(ActRangeTest, ActRangeNoDyntype) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" actlimited="true" actrange="-1 1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(),
HasSubstr("actrange specified but dyntype is 'none'"));
}
TEST_F(ActRangeTest, ActRangeDefaultsPropagate) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<default>
<general dyntype="integrator" actlimited="true" actrange="-1 1"/>
<default class="dclass">
<general actlimited="false" actrange="2 3"/>
</default>
</default>
<worldbody>
<body>
<joint name="slide" type="slide" axis="1 0 0"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<general joint="slide"/>
<general joint="slide" class="dclass"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
// first actuator
EXPECT_THAT(model->actuator_actlimited[0], 1);
EXPECT_THAT(model->actuator_actrange[0], -1);
EXPECT_THAT(model->actuator_actrange[1], 1);
// // second actuator
EXPECT_THAT(model->actuator_actlimited[1], 0);
EXPECT_THAT(model->actuator_actrange[2], 2);
EXPECT_THAT(model->actuator_actrange[3], 3);
mj_deleteModel(model);
}
// ---------------------------- test actdim ------------------------------------
using ActDimTest = MujocoTest;
TEST_F(ActDimTest, BiggerThanOneOnlyForUser) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" actdim="2"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(),
HasSubstr("actdim > 1 is only allowed for dyntype 'user'"));
}
TEST_F(ActDimTest, NonzeroNotAllowedInStateless) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" actdim="1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid actdim 1 in stateless"));
EXPECT_THAT(error.data(), HasSubstr("line 10"));
}
TEST_F(ActDimTest, ZeroNotAllowedInStateful) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="hinge"/>
</body>
</worldbody>
<actuator>
<general joint="hinge" dyntype="filter" actdim="0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid actdim 0 in stateful"));
EXPECT_THAT(error.data(), HasSubstr("line 10"));
}
// ------------- test nuser_xxx fields -----------------------------------------
using UserDataTest = MujocoTest;
TEST_F(UserDataTest, NBodyTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<size nuser_body="2"/>
<worldbody>
<body user="1 2 3"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("nuser_body"));
EXPECT_THAT(error.data(), HasSubstr("line 5"));
}
TEST_F(UserDataTest, NJointTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<size nuser_jnt="2"/>
<worldbody>
<body>
<geom size="1"/>
<joint user="1 2 3"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("nuser_jnt"));
EXPECT_THAT(error.data(), HasSubstr("line 7"));
}
TEST_F(UserDataTest, NGeomTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<size nuser_geom="2"/>
<worldbody>
<geom size="1" user="1 2 3"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("nuser_geom"));
EXPECT_THAT(error.data(), HasSubstr("line 5"));
}
TEST_F(UserDataTest, NSiteTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<size nuser_site="2"/>
<worldbody>
<site user="1 2 3"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("nuser_site"));
EXPECT_THAT(error.data(), HasSubstr("line 5"));
}
TEST_F(UserDataTest, NCameraTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<size nuser_cam="2"/>
<worldbody>
<camera user="1 2 3"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("nuser_cam"));
EXPECT_THAT(error.data(), HasSubstr("line 5"));
}
TEST_F(UserDataTest, NTendonTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<size nuser_tendon="2"/>
<worldbody>
<site name="a"/>
<site name="b"/>
</worldbody>
<tendon>
<spatial user="1 2 3">
<site site="a"/>
<site site="b"/>
</spatial>
</tendon>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("nuser_tendon"));
EXPECT_THAT(error.data(), HasSubstr("line 9"));
}
TEST_F(UserDataTest, NActuatorTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<size nuser_actuator="2"/>
<worldbody>
<body>
<geom size="1"/>
<joint name="a"/>
</body>
</worldbody>
<actuator>
<motor joint="a" user="1 2 3"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("nuser_actuator"));
EXPECT_THAT(error.data(), HasSubstr("line 11"));
}
TEST_F(UserDataTest, NSensorTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<size nuser_sensor="2"/>
<worldbody>
<site name="a"/>
</worldbody>
<sensor>
<accelerometer site="a" user="1 2 3"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("nuser_sensor"));
EXPECT_THAT(error.data(), HasSubstr("line 8"));
}
// ------------- test for auto parsing of *limited fields ----------------------
using LimitedTest = MujocoTest;
constexpr char kKeyAutoLimits[] = "user/testdata/auto_limits.xml";
// check joint limit values when automatically inferred based on range
TEST_F(LimitedTest, JointLimited) {
const string path = GetTestDataFilePath(kKeyAutoLimits);
std::array<char, 1024> err;
mjModel* model = mj_loadXML(path.c_str(), nullptr, err.data(), err.size());
ASSERT_THAT(model, NotNull()) << err.data();
// see `user/testdata/auto_limits.xml` for expected values
for (int i = 0; i < model->njnt; i++) {
EXPECT_EQ(model->jnt_limited[i], (mjtByte)model->jnt_user[i])
<< i << " " << (int)model->jnt_limited[i] << " "
<< (int)model->jnt_user[i];
}
mj_deleteModel(model);
}
TEST_F(LimitedTest, ErrorIfLimitedMissingOnJoint) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="false"/>
<worldbody>
<body>
<joint user="1" range="0 1"/>
<geom size="1"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("limited"));
EXPECT_THAT(error.data(), HasSubstr("line 6"));
}
TEST_F(LimitedTest, ExplicitLimitedFalseIsOk) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="false"/>
<worldbody>
<body>
<joint user="1" limited="false" range="0 1"/>
<geom size="1"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->jnt_limited[0], 0);
mj_deleteModel(model);
}
TEST_F(LimitedTest, ErrorIfLimitedMissingOnTendon) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="false"/>
<worldbody>
<body>
<joint type="slide"/>
<geom size="1"/>
<site name="s1"/>
</body>
<site name="s2"/>
</worldbody>
<tendon>
<spatial range="-1 1">
<site site="s1"/>
<site site="s2"/>
</spatial>
</tendon>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("limited"));
EXPECT_THAT(error.data(), HasSubstr("tendon"));
EXPECT_THAT(error.data(), HasSubstr("line 13"));
}
TEST_F(LimitedTest, ErrorIfForceLimitedMissingOnActuator) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="false"/>
<worldbody>
<body>
<joint type="slide" name="J1"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<position joint="J1" forcerange="0 100"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("forcelimited"));
EXPECT_THAT(error.data(), HasSubstr("forcerange"));
EXPECT_THAT(error.data(), HasSubstr("actuator"));
EXPECT_THAT(error.data(), HasSubstr("line 11"));
}
// ------------- tests for tendon ----------------------------------------------
using TendonTest = MujocoTest;
TEST_F(TendonTest, SiteBetweenPulleyNotAllowed) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site name="1"/>
<site name="2"/>
<site name="3"/>
</worldbody>
<tendon>
<spatial>
<site site="1"/>
<pulley divisor="1"/>
<site site="3"/>
</spatial>
</tendon>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("needs a neighbor that is not a pulley"));
EXPECT_THAT(error.data(), HasSubstr("line 9"));
}
TEST_F(TendonTest, ActuatorForceRangeNotAllowed) {
std::string xml = R"(
<mujoco>
<worldbody>
<site name="site0"/>
<site name="site1"/>
</worldbody>
<tendon>
<spatial name="spatial" actuatorfrclimited="true" actuatorfrcrange="{}">
<site site="site0"/>
<site site="site1"/>
</spatial>
</tendon>
<actuator>
<motor tendon="spatial"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
std::string str_replace = "{}";
size_t rng_ind = xml.find(str_replace);
std::string xml0 = xml;
std::string range0 = "-2 -1";
xml0.replace(rng_ind, str_replace.length(), range0);
mjModel* m0 = LoadModelFromString(xml0.c_str(), error.data(), error.size());
EXPECT_THAT(m0, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon"));
std::string xml1 = xml;
std::string range1 = "1 2";
xml1.replace(rng_ind, str_replace.length(), range1);
mjModel* m1 = LoadModelFromString(xml1.c_str(), error.data(), error.size());
EXPECT_THAT(m1, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon"));
std::string xml2 = xml;
std::string range2 = "1 0";
xml2.replace(rng_ind, str_replace.length(), range2);
mjModel* m2 = LoadModelFromString(xml2.c_str(), error.data(), error.size());
EXPECT_THAT(m2, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon"));
}
// ------------- tests for tendon springrange ----------------------------------
using SpringrangeTest = MujocoTest;
TEST_F(SpringrangeTest, DefaultsPropagate) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<tendon springlength=".2 .5"/>
</default>
<worldbody>
<site name="0"/>
<site name="1" pos="1 0 0"/>
</worldbody>
<tendon>
<spatial>
<site site="0"/>
<site site="1"/>
</spatial>
</tendon>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_MJTNUM_EQ(model->tendon_lengthspring[0], .2);
EXPECT_MJTNUM_EQ(model->tendon_lengthspring[1], .5);
mj_deleteModel(model);
}
TEST_F(SpringrangeTest, InvalidRange) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site name="0"/>
<site name="1" pos="1 0 0"/>
</worldbody>
<tendon>
<spatial springlength="1 0">
<site site="0"/>
<site site="1"/>
</spatial>
</tendon>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid springlength in tendon"));
EXPECT_THAT(error.data(), HasSubstr("line 9"));
}
using UserObjectsTest = MujocoTest;
// ------------- test frame ----------------------------------------------------
TEST_F(UserObjectsTest, Frame) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<frame euler="0 0 30">
<geom name="0" size=".1" euler="0 0 20"/>
</frame>
<frame axisangle="0 1 0 90">
<frame axisangle="0 0 1 90">
<geom name="1" size=".1"/>
</frame>
</frame>
<frame pos="0 1 0" euler="0 20 0">
<geom name="2" pos=".5 .6 .7" size=".1" euler="30 0 0"/>
</frame>
<body>
<frame pos="0 1 0">
<geom name="3" size=".1" pos="0 1 0"/>
<body pos="1 0 0">
<geom name="4" size=".1" pos="0 0 1"/>
</body>
</frame>
</body>
<body>
<geom name="5" size=".1"/>
<frame euler="90 0 0">
<joint type="hinge" axis="0 0 1"/>
</frame>
</body>
<body pos="0 1 0" euler="0 20 0">
<geom name="6" pos=".5 .6 .7" size=".1" euler="30 0 0"/>
</body>
</worldbody>
</mujoco>
)";
const mjtNum eps = MjTol(1e-14, 1e-5);
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, testing::NotNull()) << error.data();
EXPECT_EQ(m->nbody, 5);
// geom quat transformed to euler = 0 0 50
EXPECT_NEAR(m->geom_quat[0], mju_cos(25. * mjPI / 180.), 1e-3);
EXPECT_NEAR(m->geom_quat[1], 0, 0);
EXPECT_NEAR(m->geom_quat[2], 0, 0);
EXPECT_NEAR(m->geom_quat[3], mju_sin(25. * mjPI / 180.), 1e-3);
// geom transformed to frame 0 1 0, 0 0 1, 1 0 0
EXPECT_NEAR(m->geom_quat[4], .5, eps);
EXPECT_NEAR(m->geom_quat[5], .5, eps);
EXPECT_NEAR(m->geom_quat[6], .5, eps);
EXPECT_NEAR(m->geom_quat[7], .5, eps);
// geom pos transformed from 0 1 0 to 0 2 0
EXPECT_EQ(m->geom_pos[ 9], 0);
EXPECT_EQ(m->geom_pos[10], 2);
EXPECT_EQ(m->geom_pos[11], 0);
// body pos transformed from 1 0 0 to 1 1 0
EXPECT_EQ(m->body_pos[6], 1);
EXPECT_EQ(m->body_pos[7], 1);
EXPECT_EQ(m->body_pos[8], 0);
// nested geom pos not transformed
EXPECT_EQ(m->geom_pos[12], 0);
EXPECT_EQ(m->geom_pos[13], 0);
EXPECT_EQ(m->geom_pos[14], 1);
// joint axis transformed to 0 -1 0
EXPECT_NEAR(m->jnt_axis[0], 0, eps);
EXPECT_NEAR(m->jnt_axis[1], -1, eps);
EXPECT_NEAR(m->jnt_axis[2], 0, eps);
mjData* d = mj_makeData(m);
mj_kinematics(m, d);
// body and frame equivalence geom 2 vs 6
EXPECT_NEAR(d->geom_xpos[6], d->geom_xpos[18], eps);
EXPECT_NEAR(d->geom_xpos[7], d->geom_xpos[19], eps);
EXPECT_NEAR(d->geom_xpos[8], d->geom_xpos[20], eps);
EXPECT_NEAR(d->geom_xmat[18], d->geom_xmat[54], eps);
EXPECT_NEAR(d->geom_xmat[19], d->geom_xmat[55], eps);
EXPECT_NEAR(d->geom_xmat[20], d->geom_xmat[56], eps);
EXPECT_NEAR(d->geom_xmat[21], d->geom_xmat[57], eps);
mj_deleteModel(m);
mj_deleteData(d);
}
TEST_F(UserObjectsTest, FrameTransformsLight) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<frame euler="0 45 0" pos="0 0 1">
<light pos="-1 0 0" dir="1 0 -1"/>
</frame>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, NotNull()) << error.data();
EXPECT_EQ(m->nlight, 1);
const mjtNum eps = MjTol(1e-14, 1e-5);
EXPECT_NEAR(m->light_pos[0], -mju_sqrt(.5), eps);
EXPECT_NEAR(m->light_pos[1], 0, eps);
EXPECT_NEAR(m->light_pos[2], 1 + mju_sqrt(.5), eps);
EXPECT_NEAR(m->light_dir[0], 0, eps);
EXPECT_NEAR(m->light_dir[1], 0, eps);
EXPECT_NEAR(m->light_dir[2], -1, eps);
mj_deleteModel(m);
}
TEST_F(ContentTypeTest, ImageLightsReferenceTexture) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture name="texture" type="cube" builtin="flat" mark="cross" width="8"
rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/>
</asset>
<worldbody>
<light type="image" texture="texture"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, NotNull());
EXPECT_EQ(m->ntex, 1);
EXPECT_EQ(m->nlight, 1);
EXPECT_THAT(m->light_texid[0], 0);
mj_deleteModel(m);
}
// ------------- test bvh ------------------------------------------------------
TEST_F(UserObjectsTest, RobustBVH) {
static constexpr char xml1[] = R"(
<mujoco>
<worldbody>
<body>
<geom size=".1" pos="0 0 0"/>
<geom size=".1" pos="0 1 0"/>
<geom size=".1" pos="1 0 0"/>
<geom size=".1" pos="1 1 0"/>
<geom size=".1" pos="2 0 0"/>
<geom size=".1" pos="2 1 0"/>
</body>
</worldbody>
</mujoco>
)";
static constexpr char xml2[] = R"(
<mujoco>
<worldbody>
<body>
<geom size=".1" pos="0 0 0"/>
<geom size=".1" pos="0 1 0"/>
<geom size=".1" pos="1.00000000000001 0 0"/>
<geom size=".1" pos="1 1.00000000000001 0"/>
<geom size=".1" pos="2 0 0"/>
<geom size=".1" pos="2 1 0"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m1 = LoadModelFromString(xml1, error.data(), error.size());
EXPECT_THAT(m1, NotNull()) << error.data();
mjModel* m2 = LoadModelFromString(xml2, error.data(), error.size());
EXPECT_THAT(m2, NotNull()) << error.data();
EXPECT_EQ(m1->nbvh, m2->nbvh);
for (int i = 0; i < m1->nbvh; i++) {
EXPECT_EQ(m1->bvh_nodeid[i], m2->bvh_nodeid[i]);
}
mj_deleteModel(m1);
mj_deleteModel(m2);
}
// ------------- test equality compilation -------------------------------------
TEST_F(UserObjectsTest, BadConnect) {
string base = R"(
<mujoco>
<worldbody>
<site name="0" size="1"/>
<body name="1" pos="0 0 1">
<freejoint/>
<geom size="1"/>
<site name="1"/>
</body>
</worldbody>
<equality>
CONNECT
</equality>
</mujoco>
)";
int pos = base.find("CONNECT");
int len = 7;
// good model using body semantic
string xml = base.replace(pos, len, "<connect body1='1' anchor='0 0 1'/>");
char error[1024];
mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 1, 0, 0, 2));
mj_deleteModel(m);
// good model using site semantic
xml = base.replace(pos, len, "<connect site1='0' site2='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 0, 0, 0, 0));
mj_deleteModel(m);
char error_missing[] =
"either both body1 and anchor must be defined,"
" or both site1 and site2 must be defined\nElement 'connect', line 12";
// bad model (missing anchor)
xml = base.replace(pos, len, "<connect body1='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_missing));
char error_mixed[] =
"body and site semantics cannot be mixed"
"\nElement 'connect', line 12";
// bad model (mixing body and site)
xml = base.replace(pos, len, "<connect body1='1' site1='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_mixed));
// load spec with no constraints
xml = base.erase(pos, len);
mjSpec* s = mj_parseXMLString(xml.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(s, NotNull()) << error;
// add a connect but don't set objtype
mjsEquality* equality = mjs_addEquality(s, nullptr);
equality->type = mjEQ_CONNECT;
mjs_setString(equality->name1, "0");
mjs_setString(equality->name2, "1");
// expect compilation to fail
m = mj_compile(s, nullptr);
ASSERT_THAT(m, IsNull());
EXPECT_THAT(mjs_getError(s),
HasSubstr("connect constraint supports only sites and bodies"));
// set objtype, expect compilation to succeed
equality->objtype = mjOBJ_SITE;
m = mj_compile(s, nullptr);
ASSERT_THAT(m, NotNull()) << mjs_getError(s);
mj_deleteModel(m);
mj_deleteSpec(s);
}
TEST_F(UserObjectsTest, BadWeld) {
string base = R"(
<mujoco>
<worldbody>
<site name="0" size="1"/>
<body name="1" pos="0 0 1">
<freejoint/>
<geom size="1"/>
<site name="1"/>
</body>
</worldbody>
<equality>
WELD
</equality>
</mujoco>
)";
int pos = base.find("WELD");
int len = 4;
// good model using body semantic
string xml = base.replace(pos, len,
"<weld body1='1' anchor='0 0 1' torquescale='2'/>");
char error[1024];
mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
EXPECT_THAT(AsVector(m->eq_data, 10),
ElementsAre(0, 0, 1, 0, 0, 0, 1, 0, 0, 0));
mj_deleteModel(m);
// good model using site semantic
xml = base.replace(pos, len, "<weld site1='0' site2='1' torquescale='2'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
EXPECT_THAT(AsVector(m->eq_data, 10),
ElementsAre(0, 1, 0, 0, 0, 0, 0, 0, 0, 0));
mj_deleteModel(m);
char error_mixed[] =
"body and site semantics cannot be mixed"
"\nElement 'weld', line 12";
// bad model (mixing body and site)
xml = base.replace(pos, len, "<weld body1='1' site1='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_mixed));
// bad model (mixing site and anchor)
xml = base.replace(pos, len, "<weld anchor='0 0 1' site1='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_mixed));
// bad model (mixing site and relpose)
xml = base.replace(pos, len, "<weld relpose='0 0 0 1 0 0 0' site1='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_mixed));
// bad model (body and site semantics are valid, but both are specified)
xml = base.replace(pos, len, "<weld body1='1' site1='1' site2='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_mixed));
char error_underspecified[] =
"either body1 must be defined and optionally {body2, anchor, "
"relpose}, or site1 and site2 must be defined\nElement 'weld', line 12";
// bad model (underspecified body semantics)
xml = base.replace(pos, len, "<weld anchor='0 0 1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_underspecified));
// bad model (underspecified site semantics)
xml = base.replace(pos, len, "<weld site2='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_underspecified));
// load spec with no constraints
xml = base.erase(pos, len);
mjSpec* s = mj_parseXMLString(xml.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(s, NotNull()) << error;
// add a weld but don't set objtype
mjsEquality* equality = mjs_addEquality(s, nullptr);
equality->type = mjEQ_WELD;
mjs_setString(equality->name1, "0");
mjs_setString(equality->name2, "1");
// expect compilation to fail
m = mj_compile(s, nullptr);
ASSERT_THAT(m, IsNull());
EXPECT_THAT(mjs_getError(s),
HasSubstr("weld constraint supports only sites and bodies"));
// set objtype, expect compilation to succeed
equality->objtype = mjOBJ_SITE;
m = mj_compile(s, nullptr);
ASSERT_THAT(m, NotNull()) << mjs_getError(s);
mj_deleteModel(m);
mj_deleteSpec(s);
}
TEST_F(UserObjectsTest, Inertial) {
string xml = R"(
<mujoco>
<compiler angle="radian"/>
<worldbody>
<body>
<inertial mass="1" pos="2 3 4" euler="3 4 5" diaginertia="4 5 6"/>
</body>
<body>
<inertial mass="2" pos="1 2 3" fullinertia="4 3 2 0 0 0"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
EXPECT_EQ(m->body_mass[1], 1);
EXPECT_THAT(AsVector(m->body_ipos + 3, 3), ElementsAre(2, 3, 4));
EXPECT_THAT(AsVector(m->body_inertia + 3, 3), ElementsAre(4, 5, 6));
mjtNum quat[4];
const mjtNum euler[3] = {3, 4, 5};
mju_euler2Quat(quat, euler, "xyz");
EXPECT_THAT(AsVector(m->body_iquat + 4, 4),
Pointwise(MjNear(1e-8, 1e-6), AsVector(quat, 4)));
EXPECT_EQ(m->body_mass[2], 2);
EXPECT_THAT(AsVector(m->body_ipos + 6, 3), ElementsAre(1, 2, 3));
EXPECT_THAT(AsVector(m->body_inertia + 6, 3), ElementsAre(4, 3, 2));
mj_deleteModel(m);
string bad_xml1 = R"(
<mujoco>
<worldbody>
<body>
<inertial mass="1" pos="0 0 0" diaginertia="4 5 6" fullinertia="4 3 2 0 0 0"/>
</body>
</worldbody>
</mujoco>
)";
m = LoadModelFromString(bad_xml1.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr("fullinertia and diagonal inertia cannot both"));
string bad_xml2 = R"(
<mujoco>
<worldbody>
<body>
<inertial mass="1" pos="0 0 0" euler="4 5 6" fullinertia="4 3 2 0 0 0"/>
</body>
</worldbody>
</mujoco>
)";
m = LoadModelFromString(bad_xml2.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr("fullinertia and inertial orientation cannot"));
}
TEST_F(UserObjectsTest, ZeroMass) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<body>
<geom mass="0" size="0.01"/>
<geom mass="0" size="0.01"/>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
EXPECT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
// ------------- test Octree SDF computation -----------------------------------
using OctreeSDFTest = MujocoTest;
TEST_F(OctreeSDFTest, SphereSDF) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="sphere" builtin="supersphere" params="16 1 1"/>
</asset>
<worldbody>
<geom name="sdf_sphere" type="sdf" mesh="sphere"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
mjData* data = mj_makeData(model);
ASSERT_THAT(data, NotNull());
EXPECT_GT(model->nmesh, 0);
EXPECT_EQ(model->geom_type[0], mjGEOM_SDF);
int geom_id = 0;
int mesh_id = model->geom_dataid[geom_id];
mjSDF sdf;
const mjpPlugin* null_plugin = nullptr;
sdf.plugin = &null_plugin;
sdf.id = &mesh_id;
sdf.type = mjSDFTYPE_SINGLE;
sdf.geomtype = (mjtGeom*)(model->geom_type + geom_id);
// Analytic SDF for unit sphere: distance = |p| - 1
auto analyticSdf = [](const mjtNum* p) -> double {
return mju_sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2]) - 1.0;
};
int sign_errors = 0;
int total_points = 0;
double sum_sq_error = 0.0;
// Test grid of points
for (double x = -2.0; x <= 2.0; x += 0.5) {
for (double y = -2.0; y <= 2.0; y += 0.5) {
for (double z = -2.0; z <= 2.0; z += 0.5) {
mjtNum p[3] = {(mjtNum)x, (mjtNum)y, (mjtNum)z};
double sdf_dist = mjc_distance(model, data, &sdf, p);
double gt_dist = analyticSdf(p);
if ((sdf_dist < 0) != (gt_dist < 0)) {
sign_errors++;
}
double error = sdf_dist - gt_dist;
sum_sq_error += error * error;
total_points++;
}
}
}
double rmse = mju_sqrt(sum_sq_error / total_points);
EXPECT_LT(sign_errors, total_points / 200)
<< "No more than 0.5% of points should have sign errors";
EXPECT_LT(rmse, 0.11) << "RMSE should be less than 0.11";
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(OctreeSDFTest, TorusSDF) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="torus" builtin="supertorus" params="16 0.3 1 1"/>
</asset>
<worldbody>
<geom name="sdf_torus" type="sdf" mesh="torus"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
mjData* data = mj_makeData(model);
ASSERT_THAT(data, NotNull());
EXPECT_GT(model->nmesh, 0);
EXPECT_EQ(model->geom_type[0], mjGEOM_SDF);
int geom_id = 0;
int mesh_id = model->geom_dataid[geom_id];
mjSDF sdf;
const mjpPlugin* null_plugin = nullptr;
sdf.plugin = &null_plugin;
sdf.id = &mesh_id;
sdf.type = mjSDFTYPE_SINGLE;
sdf.geomtype = (mjtGeom*)(model->geom_type + geom_id);
// Analytic SDF for torus: distance = |p_proj| - r, where p_proj is
// projection of p onto circle of radius R, and r is minor radius.
// R=1, r=0.3
auto analyticSdf = [](const mjtNum* p) -> double {
double xy = mju_sqrt(p[0]*p[0] + p[1]*p[1]);
double vec[2] = {xy - 1.0, p[2]};
return mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1]) - 0.3;
};
int sign_errors = 0;
int total_points = 0;
double sum_sq_error = 0.0;
// Test grid of points
for (mjtNum x = -2.0; x <= 2.0; x += 0.5) {
for (mjtNum y = -2.0; y <= 2.0; y += 0.5) {
for (mjtNum z = -2.0; z <= 2.0; z += 0.5) {
mjtNum p[3] = {x, y, z};
double sdf_dist = mjc_distance(model, data, &sdf, p);
double gt_dist = analyticSdf(p);
if ((sdf_dist < 0) != (gt_dist < 0)) {
sign_errors++;
}
double error = sdf_dist - gt_dist;
sum_sq_error += error * error;
total_points++;
}
}
}
double rmse = mju_sqrt(sum_sq_error / total_points);
EXPECT_LT(sign_errors, total_points / 20)
<< "No more than 5% of points should have sign errors";
EXPECT_LT(rmse, 0.52) << "RMSE should be close to 0.516";
mj_deleteData(data);
mj_deleteModel(model);
}
// ------------- test actuator damping/armature validation ---------------------
using ActuatorTransmissionTest = MujocoTest;
TEST_F(ActuatorTransmissionTest, DampingRejectsSiteTransmission) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide"/>
<geom size="1"/>
<site name="s"/>
</body>
</worldbody>
<actuator>
<general site="s" damping="1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error.data(), HasSubstr("damping requires joint or tendon"));
}
TEST_F(ActuatorTransmissionTest, ArmatureRejectsSiteTransmission) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint type="slide"/>
<geom size="1"/>
<site name="s"/>
</body>
</worldbody>
<actuator>
<general site="s" armature="1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error.data(), HasSubstr("armature requires joint or tendon"));
}
} // namespace
} // namespace mujoco