Loading files
This 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_api.cc.
#include <array>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <filesystem> // NOLINT
#include <functional>
#include <map>
#include <memory>
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <absl/strings/str_format.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjspec.h>
#include <mujoco/mujoco.h>
#include "src/xml/xml_api.h"
#include "test/compare_model.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::NotNull;
// -------------------------- test model manipulation -------------------------
TEST_F(MujocoTest, GetSetData) {
mjSpec* spec = mj_makeSpec();
mjsBody* world = mjs_findBody(spec, "world");
mjsBody* body = mjs_addBody(world, 0);
mjsSite* site = mjs_addSite(body, 0);
{
double vec[10] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
const char* str = "sitename";
mjs_setName(site->element, str);
mjs_setDouble(site->userdata, vec, 10);
}
EXPECT_THAT(mjs_getName(site->element)->c_str(), HasSubstr("sitename"));
int nsize;
const double* vec = mjs_getDouble(site->userdata, &nsize);
for (int i = 0; i < nsize; ++i) {
EXPECT_EQ(vec[i], i);
}
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, TreeTraversal) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="body1">
<site name="site1"/>
<body name="body2">
<site name="site4"/>
</body>
<geom name="geom1" size="1"/>
<geom name="geom2" size="1"/>
<site name="site2"/>
<site name="site3"/>
<geom name="geom3" size="1"/>
</body>
<body name="body3">
<site name="site5"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> err;
mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size());
ASSERT_THAT(spec, NotNull()) << err.data();
mjsBody* world = mjs_findBody(spec, "world");
mjsBody* body1 = mjs_findBody(spec, "body1");
mjsBody* body2 = mjs_findBody(spec, "body2");
mjsBody* body3 = mjs_findBody(spec, "body3");
mjsElement* b1 = body1->element;
mjsElement* b2 = body2->element;
mjsElement* b3 = body3->element;
mjsElement* site1 = mjs_findElement(spec, mjOBJ_SITE, "site1");
mjsElement* site2 = mjs_findElement(spec, mjOBJ_SITE, "site2");
mjsElement* site3 = mjs_findElement(spec, mjOBJ_SITE, "site3");
mjsElement* site4 = mjs_findElement(spec, mjOBJ_SITE, "site4");
mjsElement* site5 = mjs_findElement(spec, mjOBJ_SITE, "site5");
mjsElement* geom1 = mjs_findElement(spec, mjOBJ_GEOM, "geom1");
mjsElement* geom2 = mjs_findElement(spec, mjOBJ_GEOM, "geom2");
mjsElement* geom3 = mjs_findElement(spec, mjOBJ_GEOM, "geom3");
// test nonexistent
EXPECT_EQ(mjs_firstElement(spec, mjOBJ_ACTUATOR), nullptr);
EXPECT_EQ(mjs_firstElement(spec, mjOBJ_LIGHT), nullptr);
EXPECT_EQ(mjs_firstChild(body1, mjOBJ_CAMERA, /*recurse=*/true), nullptr);
EXPECT_EQ(mjs_firstChild(body1, mjOBJ_TENDON, /*recurse=*/true), nullptr);
// test first, nonrecursive
EXPECT_EQ(mjs_firstElement(spec, mjOBJ_BODY), world->element);
EXPECT_EQ(site1, mjs_firstElement(spec, mjOBJ_SITE));
EXPECT_EQ(site1, mjs_firstChild(body1, mjOBJ_SITE, /*recurse=*/false));
EXPECT_EQ(geom1, mjs_firstChild(body1, mjOBJ_GEOM, /*recurse=*/false));
EXPECT_EQ(site4, mjs_firstChild(body2, mjOBJ_SITE, /*recurse=*/false));
EXPECT_EQ(site5, mjs_firstChild(body3, mjOBJ_SITE, /*recurse=*/false));
// test first, recursive
EXPECT_EQ(site1, mjs_firstChild(world, mjOBJ_SITE, /*recurse=*/true));
EXPECT_EQ(geom1, mjs_firstChild(world, mjOBJ_GEOM, /*recurse=*/true));
EXPECT_EQ(site4, mjs_firstChild(body2, mjOBJ_SITE, /*recurse=*/true));
EXPECT_EQ(site5, mjs_firstChild(body3, mjOBJ_SITE, /*recurse=*/true));
// test next, nonrecursive
EXPECT_EQ(site2, mjs_nextChild(body1, site1, /*recursive=*/false));
EXPECT_EQ(site3, mjs_nextChild(body1, site2, /*recursive=*/false));
EXPECT_EQ(nullptr, mjs_nextChild(body1, site3, /*recursive=*/false));
EXPECT_EQ(geom2, mjs_nextChild(body1, geom1, /*recursive=*/false));
EXPECT_EQ(geom3, mjs_nextChild(body1, geom2, /*recursive=*/false));
EXPECT_EQ(nullptr, mjs_nextChild(body1, geom3, /*recursive=*/false));
EXPECT_EQ(mjs_nextElement(spec, site1), site2);
EXPECT_EQ(mjs_nextElement(spec, site2), site3);
EXPECT_EQ(mjs_nextElement(spec, site3), site4);
EXPECT_EQ(mjs_nextElement(spec, site4), site5);
EXPECT_EQ(mjs_nextElement(spec, site5), nullptr);
EXPECT_EQ(mjs_nextElement(spec, geom1), geom2);
EXPECT_EQ(mjs_nextElement(spec, geom2), geom3);
EXPECT_EQ(mjs_nextElement(spec, geom3), nullptr);
// test next, recursive
EXPECT_EQ(b2, mjs_nextChild(world, b1, /*recursive=*/true));
EXPECT_EQ(b3, mjs_nextChild(world, b2, /*recursive=*/true));
EXPECT_EQ(site2, mjs_nextChild(body1, site1, /*recursive=*/true));
EXPECT_EQ(site3, mjs_nextChild(body1, site2, /*recursive=*/true));
EXPECT_EQ(site4, mjs_nextChild(body1, site3, /*recursive=*/true));
EXPECT_EQ(site4, mjs_nextChild(world, site3, /*recursive=*/true));
EXPECT_EQ(site5, mjs_nextChild(world, site4, /*recursive=*/true));
EXPECT_EQ(nullptr, mjs_nextChild(body1, site5, /*recursive=*/true));
EXPECT_EQ(geom2, mjs_nextChild(body1, geom1, /*recursive=*/true));
EXPECT_EQ(geom3, mjs_nextChild(body1, geom2, /*recursive=*/true));
EXPECT_EQ(nullptr, mjs_nextChild(body1, geom3, /*recursive=*/true));
// check compilation ordering of sites
mjModel* model = mj_compile(spec, nullptr);
EXPECT_THAT(model, NotNull());
EXPECT_EQ(mjs_getId(site1), 0);
EXPECT_EQ(mjs_getId(site2), 1);
EXPECT_EQ(mjs_getId(site3), 2);
EXPECT_EQ(mjs_getId(site4), 3);
EXPECT_EQ(mjs_getId(site5), 4);
mj_deleteModel(model);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, AttachAndChildDeletion) {
mjSpec* child_spec = mj_makeSpec();
mjsBody* child_world = mjs_findBody(child_spec, "world");
mjsBody* child_body = mjs_addBody(child_world, 0);
mjsJoint* freejoint = mjs_addJoint(child_body, 0);
freejoint->type = mjJNT_FREE;
mjs_setName(freejoint->element, "child_freejoint");
mjSpec* parent_spec = mj_makeSpec();
mjsBody* parent_world = mjs_findBody(parent_spec, "world");
mjsBody* parent_body = mjs_addBody(parent_world, 0);
// Attach child spec to parent_body
mjsElement* attached =
mjs_attach(parent_body->element, child_spec->element, "pre_", "");
ASSERT_THAT(attached, NotNull());
// Delete freejoint from child_spec, should fail because it is attached
int result = mjs_delete(child_spec, freejoint->element);
EXPECT_EQ(result, -1);
// The freejoint should still be in parent_spec because deletion failed
mjsElement* found_joint =
mjs_findElement(parent_spec, mjOBJ_JOINT, "pre_child_freejoint");
EXPECT_THAT(found_joint, NotNull());
mj_deleteSpec(child_spec);
mj_deleteSpec(parent_spec);
}
TEST_F(MujocoTest, OriginSpecInvariantToAttachment) {
mjSpec* child_spec = mj_makeSpec();
mjsBody* child_world = mjs_findBody(child_spec, "world");
mjsBody* child_body = mjs_addBody(child_world, 0);
mjsJoint* freejoint = mjs_addJoint(child_body, 0);
freejoint->type = mjJNT_FREE;
mjs_setName(freejoint->element, "child_freejoint");
mjSpec* parent_spec = mj_makeSpec();
mjsBody* parent_world = mjs_findBody(parent_spec, "world");
mjsBody* parent_body = mjs_addBody(parent_world, 0);
mjs_setName(parent_body->element, "parent_body");
// Attach child spec to parent_body
mjsElement* attached =
mjs_attach(parent_body->element, child_spec->element, "pre_", "");
ASSERT_THAT(attached, NotNull());
// The freejoint should still be in parent_spec because deletion failed
mjsElement* child_spec_joint =
mjs_findElement(parent_spec, mjOBJ_JOINT, "pre_child_freejoint");
EXPECT_EQ(mjs_getSpec(child_spec_joint), parent_spec);
EXPECT_EQ(mjs_getOriginSpec(child_spec_joint), child_spec);
mjsElement* parent_spec_body =
mjs_findElement(parent_spec, mjOBJ_BODY, "parent_body");
EXPECT_EQ(mjs_getOriginSpec(parent_spec_body), parent_spec);
mj_deleteSpec(child_spec);
mj_deleteSpec(parent_spec);
}
int open_mock(mjResource* resource) {
static const char parent_xml[] = R"(
<mujoco>
<worldbody>
<body name="parent_body"/>
</worldbody>
</mujoco>
)";
resource->data = mju_malloc(sizeof(parent_xml));
absl::SNPrintF(static_cast<char*>(resource->data), sizeof(parent_xml), "%s",
parent_xml);
return 1;
}
int read_mock(mjResource* resource, const void** buffer) {
*buffer = resource->data;
return std::strlen((const char*)resource->data);
}
void close_mock(mjResource* resource) {
mju_free(resource->data);
resource->data = nullptr;
}
TEST_F(MujocoTest, AttachedSpecDoesNotInheritURI) {
// This test checks that when we attach a child spec to a parent spec that was
// loaded from a resource provider, the child spec does not inherit the
// resource URI from the parent. This allows the child spec to specify assets
// relative to its model file or in the VFS.
mjpResourceProvider provider = {
.prefix = "fakeprovider",
.open = open_mock,
.read = read_mock,
.close = close_mock,
};
mjp_registerResourceProvider(&provider);
std::array<char, 1024> err;
mjSpec* parent_spec =
mj_parseXML("fakeprovider:parent.xml", nullptr, err.data(), err.size());
mjs_setString(parent_spec->modelname, "parent");
ASSERT_THAT(parent_spec, NotNull()) << err.data();
// Create child spec
static constexpr char child_xml[] = R"(
<mujoco>
<worldbody>
<body name="child_body">
<geom type="mesh" mesh="asset"/>
</body>
</worldbody>
<asset>
<mesh name="asset" file="asset.obj"/>
</asset>
</mujoco>
)";
// Setup VFS with asset
mjVFS vfs;
mj_defaultVFS(&vfs);
static constexpr char asset_data[] = R"(
v 0 0 0
v 1 0 0
v 0 1 0
v 0 0 1
f 1 2 3
f 1 2 4
f 2 3 4
f 3 1 4
)";
mj_addBufferVFS(&vfs, "asset.obj", asset_data, sizeof(asset_data));
mjSpec* child_spec =
mj_parseXMLString(child_xml, &vfs, err.data(), err.size());
mjs_setString(child_spec->modelname, "child");
ASSERT_THAT(child_spec, NotNull()) << err.data();
// Attach child spec to parent spec's world body
mjsBody* world = mjs_findBody(parent_spec, "world");
ASSERT_THAT(world, NotNull());
mjsElement* attached =
mjs_attach(world->element, child_spec->element, "", "");
ASSERT_THAT(attached, NotNull());
mjModel* model = mj_compile(parent_spec, &vfs);
mj_deleteVFS(&vfs);
EXPECT_THAT(model, NotNull()) << mjs_getError(parent_spec);
if (model) {
mj_deleteModel(model);
}
mj_deleteSpec(parent_spec);
mj_deleteSpec(child_spec);
}
TEST_F(MujocoTest, ActivatePlugin) {
mjSpec* spec = mj_makeSpec();
mjs_activatePlugin(spec, "mujoco.elasticity.cable");
// associate plugin to body
mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), 0);
mjs_setString(body->plugin.plugin_name, "mujoco.elasticity.cable");
body->plugin.element = mjs_addPlugin(spec)->element;
body->plugin.active = true;
mjsGeom* geom = mjs_addGeom(body, 0);
geom->type = mjGEOM_BOX;
geom->size[0] = 1;
geom->size[1] = 1;
geom->size[2] = 1;
// compile and check that the plugin is present
mjModel* model = mj_compile(spec, NULL);
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model->nplugin, 1);
EXPECT_THAT(model->body_plugin[1], 0);
mj_deleteSpec(spec);
mj_deleteModel(model);
}
TEST_F(MujocoTest, DeletePlugin) {
mjSpec* spec = mj_makeSpec();
mjs_activatePlugin(spec, "mujoco.pid");
// create body
mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), 0);
mjsJoint* joint = mjs_addJoint(body, 0);
mjsGeom* geom = mjs_addGeom(body, 0);
mjs_setName(joint->element, "j1");
joint->type = mjJNT_SLIDE;
geom->size[0] = 1;
// add actuator
mjsActuator* actuator = mjs_addActuator(spec, 0);
mjs_setString(actuator->target, "j1");
mjs_setString(actuator->plugin.plugin_name, "mujoco.pid");
actuator->plugin.element = mjs_addPlugin(spec)->element;
actuator->plugin.active = true;
actuator->trntype = mjTRN_JOINT;
// compile and check that the plugin is present
mjModel* model = mj_compile(spec, NULL);
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model->nu, 1);
EXPECT_THAT(model->nplugin, 1);
EXPECT_THAT(model->actuator_plugin[0], 0);
// delete actuator
mjs_delete(spec, actuator->element);
// recompile and check that the plugin is not present
mjModel* newmodel = mj_compile(spec, NULL);
EXPECT_THAT(newmodel, NotNull());
EXPECT_THAT(newmodel->nu, 0);
EXPECT_THAT(newmodel->nplugin, 0);
mj_deleteSpec(spec);
mj_deleteModel(model);
mj_deleteModel(newmodel);
}
TEST_F(MujocoTest, SetToDCMotorNullable) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
double motorconst[2] = {0.05, 0.05};
double resistance = 2.0;
const char* err =
mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, nullptr,
nullptr, nullptr, nullptr, nullptr, nullptr, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gainprm[0], 2.0);
EXPECT_EQ(actuator->gainprm[1], 0.05);
EXPECT_EQ(actuator->gainprm[4], 0);
EXPECT_EQ(actuator->gainprm[5], 0);
EXPECT_EQ(actuator->gainprm[6], 0);
EXPECT_EQ(actuator->dynprm[7], 0);
EXPECT_EQ(actuator->dynprm[8], 0);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToDCMotorDeriveKe) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
double resistance = 2.0;
double nominal[3] = {12.0, 0, 100.0}; // vn=12, omega0=100
const char* err =
mjs_setToDCMotor(actuator, nullptr, resistance, nominal, nullptr, nullptr,
nullptr, nullptr, nullptr, nullptr, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gainprm[0], 2.0);
EXPECT_NEAR(actuator->gainprm[1], 0.12, 1e-5);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToDCMotorFull) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
double motorconst[2] = {0.05, 0.05};
double resistance = 2.0;
double saturation[3] = {1.0, 2.0, 3.0};
double controller[6] = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0};
const char* err =
mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, saturation,
nullptr, nullptr, controller, nullptr, nullptr, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gainprm[0], 2.0); // resistance
EXPECT_EQ(actuator->gainprm[1], 0.05); // K
EXPECT_EQ(actuator->gainprm[4], 10.0); // kp
EXPECT_EQ(actuator->gainprm[5], 20.0); // ki
EXPECT_EQ(actuator->gainprm[6], 30.0); // kd
EXPECT_EQ(actuator->dynprm[7], 40.0); // slewmax
EXPECT_EQ(actuator->dynprm[8], 50.0); // Imax
EXPECT_EQ(actuator->gainprm[7], 60.0); // Vmax
EXPECT_EQ(actuator->dynprm[1], 3.0); // (di/dt)_max
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToDCMotorLuGre) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
double motorconst[2] = {0.05, 0.05};
double resistance = 2.0;
double lugre[5] = {100.0, 1.0, 0.5, 0.7, 10.0};
const char* err =
mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, nullptr,
nullptr, nullptr, nullptr, nullptr, lugre, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->dynprm[5], 100.0); // stiffness
EXPECT_EQ(actuator->dynprm[6], 1.0); // damping
EXPECT_EQ(actuator->biasprm[3], 0.5); // coulomb
EXPECT_EQ(actuator->biasprm[4], 0.7); // static
EXPECT_EQ(actuator->biasprm[5], 10.0); // stribeck
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToOrientation) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
// kv variant, default (expmap) chart
double kv = 2.0;
const char* err = mjs_setToOrientation(actuator, 5.0, &kv, nullptr, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gaintype, mjGAIN_SO3);
EXPECT_EQ(actuator->biastype, mjBIAS_SO3);
EXPECT_EQ(actuator->dyntype, mjDYN_NONE);
EXPECT_EQ(actuator->gainprm[0], 5.0);
EXPECT_EQ(actuator->biasprm[1], -5.0);
EXPECT_EQ(actuator->biasprm[2], -2.0);
EXPECT_EQ(actuator->ctrlspec, 0);
// dampratio variant, quat chart
double dampratio = 1.0;
err = mjs_setToOrientation(actuator, 5.0, nullptr, &dampratio, mjCHART_QUAT);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->biasprm[2], 1.0);
EXPECT_EQ(actuator->ctrlspec, mjCHART_QUAT);
// kv and dampratio are mutually exclusive
err = mjs_setToOrientation(actuator, 5.0, &kv, &dampratio, 0);
EXPECT_STREQ(err, "kv and dampratio cannot both be defined");
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetToPID) {
mjSpec* spec = mj_makeSpec();
mjsActuator* actuator = mjs_addActuator(spec, 0);
// stateless PID with kv, default input signature
double kv = 3.0;
const char* err = mjs_setToPID(actuator, 5.0, &kv, nullptr, nullptr, nullptr,
nullptr, 0, 0);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gaintype, mjGAIN_PID);
EXPECT_EQ(actuator->biastype, mjBIAS_AFFINE);
EXPECT_EQ(actuator->dyntype, mjDYN_NONE);
EXPECT_EQ(actuator->biasprm[1], -5.0);
EXPECT_EQ(actuator->biasprm[2], -3.0);
EXPECT_EQ(actuator->gainprm[0], 0.0);
// integral action with anti-windup, pos-only signature
double ki = 0.5, imax = 2.0, dampratio = 1.0;
err = mjs_setToPID(actuator, 5.0, nullptr, &dampratio, &ki, &imax, nullptr, 0,
mjINPUT_POS);
EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->dyntype, mjDYN_PID);
EXPECT_EQ(actuator->gainprm[0], 0.5);
EXPECT_EQ(actuator->dynprm[0], 2.0);
EXPECT_EQ(actuator->biasprm[2], 1.0);
EXPECT_EQ(actuator->ctrlspec, mjINPUT_POS);
// kv and dampratio are mutually exclusive
err = mjs_setToPID(actuator, 5.0, &kv, &dampratio, nullptr, nullptr, nullptr,
0, 0);
EXPECT_STREQ(err, "kv and dampratio cannot both be defined");
mj_deleteSpec(spec);
}
static constexpr char xml_plugin_1[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>
<body name="body"/>
</worldbody>
</mujoco>)";
static constexpr char xml_plugin_2[] = R"(
<mujoco model="MuJoCo Model">
<extension>
<plugin plugin="mujoco.pid">
<instance name="actuator-1">
<config key="ki" value="4.0"/>
<config key="slewmax" value="3.14159"/>
</instance>
</plugin>
</extension>
<worldbody>
<body name="empty"/>
<body name="body">
<joint name="joint"/>
<geom size="0.1"/>
</body>
</worldbody>
<actuator>
<plugin name="actuator-1" plugin="mujoco.pid" instance="actuator-1"
joint="joint" actdim="2"/>
</actuator>
</mujoco>)";
TEST_F(MujocoTest, AttachPlugin) {
std::array<char, 1000> err;
mjSpec* parent = mj_parseXMLString(xml_plugin_1, 0, err.data(), err.size());
ASSERT_THAT(parent, NotNull()) << err.data();
mjSpec* spec_1 = mj_parseXMLString(xml_plugin_2, 0, err.data(), err.size());
ASSERT_THAT(spec_1, NotNull()) << err.data();
// do a copy before attaching
mjSpec* spec_2 = mj_copySpec(spec_1);
mjs_setString(spec_2->modelname, "first_copy");
mjSpec* spec_3 = mj_copySpec(spec_1);
mjs_setString(spec_3->modelname, "second_copy");
ASSERT_THAT(spec_3, NotNull()) << err.data();
// attach a body referencing the plugin to the frame and compile
mjsBody* body_1 = mjs_findBody(parent, "body");
EXPECT_THAT(body_1, NotNull());
mjsFrame* attachment_frame = mjs_addFrame(body_1, 0);
EXPECT_THAT(attachment_frame, NotNull());
mjs_attach(attachment_frame->element, mjs_findBody(spec_1, "body")->element,
"child-", "");
mjModel* model_1 = mj_compile(parent, nullptr);
EXPECT_THAT(model_1, NotNull());
EXPECT_THAT(model_1->nbody, 3);
// attach it a second time to test namespacing and compile
ASSERT_THAT(spec_2, NotNull()) << err.data();
mjs_attach(attachment_frame->element, mjs_findBody(spec_2, "body")->element,
"copy-", "");
mjModel* model_2 = mj_compile(parent, nullptr);
EXPECT_THAT(model_2, NotNull());
EXPECT_THAT(model_2->nbody, 4);
// attach a body not referencing the plugin and compile
mjs_attach(attachment_frame->element, mjs_findBody(spec_3, "empty")->element,
"empty-", "");
mjModel* model_3 = mj_compile(parent, nullptr);
EXPECT_THAT(model_3, NotNull());
EXPECT_THAT(model_3->nbody, 5);
mj_deleteModel(model_1);
mj_deleteModel(model_2);
mj_deleteModel(model_3);
mj_deleteSpec(parent);
mj_deleteSpec(spec_1);
mj_deleteSpec(spec_2);
mj_deleteSpec(spec_3);
}
TEST_F(MujocoTest, DetachPlugin) {
std::array<char, 1000> err;
mjSpec* parent = mj_parseXMLString(xml_plugin_1, 0, err.data(), err.size());
ASSERT_THAT(parent, NotNull()) << err.data();
mjSpec* child = mj_parseXMLString(xml_plugin_2, 0, err.data(), err.size());
ASSERT_THAT(child, NotNull()) << err.data();
// attach a body referencing the plugin to the frame
mjsElement* frame = mjs_addFrame(mjs_findBody(parent, "world"), 0)->element;
mjsElement* body = mjs_findBody(child, "body")->element;
EXPECT_THAT(mjs_attach(frame, body, "child-", ""), NotNull());
// detach the body and compile
mjsBody* body_to_detach = mjs_findBody(parent, "child-body");
EXPECT_THAT(body_to_detach, NotNull());
EXPECT_THAT(mjs_delete(parent, body_to_detach->element), 0);
mjModel* model = mj_compile(parent, nullptr);
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model->nbody, 2);
EXPECT_THAT(model->nplugin, 0);
mj_deleteModel(model);
mj_deleteSpec(parent);
mj_deleteSpec(child);
}
TEST_F(MujocoTest, AttachExplicitPlugin) {
static constexpr char xml_parent[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>
<body name="body"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> err;
mjSpec* parent = mj_parseXMLString(xml_parent, 0, err.data(), err.size());
ASSERT_THAT(parent, NotNull()) << err.data();
mjSpec* child = mj_makeSpec();
mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), 0);
mjsGeom* geom = mjs_addGeom(body, 0);
mjsSite* site = mjs_addSite(body, 0);
mjsSensor* sensor = mjs_addSensor(child);
mjsPlugin* plugin = mjs_addPlugin(child);
mjs_activatePlugin(child, "mujoco.sensor.touch_grid");
mjs_setString(plugin->plugin_name, "mujoco.sensor.touch_grid");
mjs_setString(sensor->plugin.plugin_name, "mujoco.sensor.touch_grid");
mjs_setName(body->element, "body");
mjs_setName(sensor->element, "touch2");
mjs_setString(sensor->objname, "touch2");
mjs_setName(site->element, "touch2");
geom->size[0] = 0.1;
site->size[0] = 0.001;
sensor->type = mjSENS_PLUGIN;
sensor->objtype = mjOBJ_SITE;
sensor->plugin.element = plugin->element;
sensor->plugin.active = true;
std::map<std::string, std::string, std::less<> > config_attribs;
config_attribs["size"] = "8 12";
config_attribs["fov"] = "10 13";
config_attribs["gamma"] = "0";
config_attribs["nchannel"] = "1";
mjs_setPluginAttributes(plugin, &config_attribs);
mjsBody* body_parent = mjs_findBody(parent, "body");
EXPECT_THAT(body_parent, NotNull());
mjsFrame* attachment_frame = mjs_addFrame(body_parent, 0);
EXPECT_THAT(attachment_frame, NotNull());
mjs_attach(attachment_frame->element, mjs_findBody(child, "body")->element,
"child-", "");
mjModel* model = mj_compile(parent, nullptr);
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model->nplugin, 1);
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model);
}
TEST_F(MujocoTest, ReplicatePlugin) {
static constexpr char xml[] = R"(
<mujoco>
<extension>
<plugin plugin="mujoco.sdf.torus">
<instance name="torus" />
</plugin>
</extension>
<asset>
<mesh name="torus">
<plugin instance="torus" />
</mesh>
</asset>
<worldbody>
<geom type="sdf" mesh="torus">
<plugin instance="torus" />
</geom>
<replicate count="100">
<body />
</replicate>
</worldbody>
</mujoco>)";
std::array<char, 1000> err;
mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size());
ASSERT_THAT(spec, NotNull()) << err.data();
mjModel* model = mj_compile(spec, nullptr);
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model->nplugin, 1);
mj_deleteSpec(spec);
mj_deleteModel(model);
}
TEST_F(MujocoTest, ReplicateExplicitPlugin) {
static constexpr char xml[] = R"(
<mujoco>
<extension>
<plugin plugin="mujoco.sensor.touch_grid"/>
</extension>
<worldbody>
<body name="tactile_sensor_2">
<replicate count="2" offset="0 0.1 0">
<geom type="sphere" size=".0008" />
</replicate>
<site name="touch2" size="0.001"/>
</body>
</worldbody>
<sensor>
<plugin name="touch2" plugin="mujoco.sensor.touch_grid" objtype="site" objname="touch2">
<config key="size" value="8 12"/>
<config key="fov" value="10 13"/>
<config key="gamma" value="0"/>
<config key="nchannel" value="1"/>
</plugin>
</sensor>
</mujoco>)";
std::array<char, 1000> err;
mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size());
ASSERT_THAT(spec, NotNull()) << err.data();
mjModel* model = mj_compile(spec, nullptr);
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model->nplugin, 1);
mj_deleteSpec(spec);
mj_deleteModel(model);
}
TEST_F(MujocoTest, SetNameDetectsRepeatedNames) {
mjSpec* spec = mj_makeSpec();
mjsBody* world = mjs_findBody(spec, "world");
mjsGeom* geom1 = mjs_addGeom(world, 0);
mjsGeom* geom2 = mjs_addGeom(world, 0);
mjsSite* site = mjs_addSite(world, 0);
// repeated name within a type is an error
EXPECT_EQ(mjs_setName(geom1->element, "a"), 0);
EXPECT_EQ(mjs_setName(geom2->element, "a"), -1);
EXPECT_STREQ(mjs_getError(spec), "Error: repeated name 'a' in geom");
// same name across types is fine
EXPECT_EQ(mjs_setName(site->element, "a"), 0);
// renaming an element frees its old name
EXPECT_EQ(mjs_setName(geom1->element, "b"), 0);
EXPECT_EQ(mjs_setName(geom2->element, "a"), 0);
EXPECT_EQ(mjs_setName(geom1->element, "a"), -1);
// a failed rename leaves the previous name in place
EXPECT_STREQ(mjs_getName(geom1->element)->c_str(), "b");
// renaming to the current name is fine
EXPECT_EQ(mjs_setName(geom2->element, "a"), 0);
// empty names may repeat
EXPECT_EQ(mjs_setName(geom1->element, ""), 0);
mjsGeom* geom3 = mjs_addGeom(world, 0);
EXPECT_EQ(mjs_setName(geom3->element, ""), 0);
// deleting an element frees its name
EXPECT_EQ(mjs_delete(spec, geom2->element), 0);
EXPECT_EQ(mjs_setName(geom3->element, "a"), 0);
// frames are checked too
mjsFrame* frame1 = mjs_addFrame(world, nullptr);
mjsFrame* frame2 = mjs_addFrame(world, nullptr);
EXPECT_EQ(mjs_setName(frame1->element, "f"), 0);
EXPECT_EQ(mjs_setName(frame2->element, "f"), -1);
EXPECT_THAT(mjs_getError(spec), HasSubstr("repeated name 'f'"));
EXPECT_EQ(mjs_setName(frame2->element, "g"), 0);
// names that arrived through attach are visible to the check
mjSpec* child = mj_makeSpec();
mjsBody* child_body = mjs_addBody(mjs_findBody(child, "world"), 0);
EXPECT_EQ(mjs_setName(child_body->element, "attached"), 0);
mjsFrame* frame = mjs_addFrame(world, nullptr);
ASSERT_THAT(mjs_attach(frame->element, child_body->element, "", ""),
NotNull());
mjsBody* body = mjs_addBody(world, 0);
EXPECT_EQ(mjs_setName(body->element, "attached"), -1);
EXPECT_STREQ(mjs_getError(spec), "Error: repeated name 'attached' in body");
EXPECT_EQ(mjs_setName(body->element, "own"), 0);
// the model compiles once names are unique
geom1->size[0] = geom3->size[0] = 1;
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << mjs_getError(spec);
EXPECT_EQ(mj_name2id(model, mjOBJ_GEOM, "a"), 1);
EXPECT_EQ(mj_name2id(model, mjOBJ_BODY, "attached"), 1);
mj_deleteModel(model);
mj_deleteSpec(child);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SetNameDetectsRepeatedNamesFromXML) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="floor" type="plane" size="1 1 1"/>
<body name="box">
<geom name="boxgeom" size="0.1"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
ASSERT_THAT(spec, NotNull()) << error.data();
// names loaded from XML are visible to the check
mjsBody* world = mjs_findBody(spec, "world");
mjsGeom* geom = mjs_addGeom(world, 0);
geom->size[0] = 1;
EXPECT_EQ(mjs_setName(geom->element, "floor"), -1);
EXPECT_STREQ(mjs_getError(spec), "Error: repeated name 'floor' in geom");
EXPECT_EQ(mjs_setName(geom->element, "boxgeom"), -1);
EXPECT_EQ(mjs_setName(geom->element, "box"), 0); // body name, other type
// still checked after a compile
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << mjs_getError(spec);
mjsBody* body = mjs_addBody(world, 0);
EXPECT_EQ(mjs_setName(body->element, "box"), -1);
EXPECT_EQ(mjs_setName(body->element, "box2"), 0);
mj_deleteModel(model);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, SignatureTracksCompilation) {
mjSpec* spec = mj_makeSpec();
mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), 0);
mjsGeom* geom = mjs_addGeom(body, 0);
geom->size[0] = 1;
// an edited spec has no signature until it is compiled
EXPECT_EQ(spec->element->signature, 0);
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << mjs_getError(spec);
EXPECT_EQ(spec->element->signature, model->signature);
// adding an element clears it, and deleting the element does not bring it
// back
mjsGeom* added = mjs_addGeom(body, 0);
EXPECT_EQ(spec->element->signature, 0);
EXPECT_EQ(mjs_delete(spec, added->element), 0);
EXPECT_EQ(spec->element->signature, 0);
// recompiling restores it
mjModel* model2 = mj_compile(spec, 0);
ASSERT_THAT(model2, NotNull()) << mjs_getError(spec);
EXPECT_EQ(spec->element->signature, model2->signature);
EXPECT_EQ(model2->signature, model->signature);
// copying a compiled spec preserves the signature, so the copy still binds
mjSpec* clean_copy = mj_copySpec(spec);
EXPECT_EQ(clean_copy->element->signature, model2->signature);
// attaching clears it
mjSpec* child = mj_makeSpec();
mjsBody* child_body = mjs_addBody(mjs_findBody(child, "world"), 0);
mjsFrame* frame = mjs_addFrame(body, nullptr);
ASSERT_THAT(mjs_attach(frame->element, child_body->element, "c_", ""),
NotNull());
EXPECT_EQ(spec->element->signature, 0);
// copying an edited spec keeps it cleared
mjSpec* dirty_copy = mj_copySpec(spec);
EXPECT_EQ(dirty_copy->element->signature, 0);
mjModel* model3 = mj_compile(spec, 0);
ASSERT_THAT(model3, NotNull()) << mjs_getError(spec);
EXPECT_EQ(spec->element->signature, model3->signature);
EXPECT_NE(model3->signature, model2->signature);
mj_deleteModel(model3);
mj_deleteModel(model2);
mj_deleteModel(model);
mj_deleteSpec(dirty_copy);
mj_deleteSpec(clean_copy);
mj_deleteSpec(child);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, RecompileFails) {
mjSpec* spec = mj_makeSpec();
mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), 0);
mjsGeom* geom = mjs_addGeom(body, 0);
geom->type = mjGEOM_SPHERE;
geom->size[0] = 1;
mjModel* model = mj_compile(spec, 0);
mjData* data = mj_makeData(model);
mjsMaterial* mat1 = mjs_addMaterial(spec, 0);
mjsMaterial* mat2 = mjs_addMaterial(spec, 0);
EXPECT_EQ(mjs_setName(mat1->element, "yellow"), 0);
EXPECT_EQ(mjs_setName(mat2->element, "yellow"), -1);
EXPECT_STREQ(mjs_getError(spec), "Error: repeated name 'yellow' in material");
EXPECT_EQ(mj_recompile(spec, 0, model, data), -1);
EXPECT_THAT(mjs_getError(spec), HasSubstr("empty name in material"));
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, ModifyShellInertiaFails) {
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>
</worldbody>
</mujoco>
)";
std::array<char, 1000> err;
mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size());
ASSERT_THAT(spec, NotNull()) << err.data();
// add a geom to spec
mjsGeom* geom = mjs_addGeom(mjs_findBody(spec, "world"), nullptr);
geom->type = mjGEOM_MESH;
mjs_setString(geom->meshname, "example_mesh");
geom->typeinertia = mjINERTIA_SHELL;
mjModel* model = mj_compile(spec, nullptr);
EXPECT_THAT(model, IsNull());
EXPECT_THAT(mjs_getError(spec),
HasSubstr("inertia should be specified in the mesh asset"));
mj_deleteSpec(spec);
mj_deleteModel(model);
}
// the bounding volume hierarchy of a body is rebuilt by every compilation
TEST_F(MujocoTest, RecompileEditGeoms) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="body">
<geom name="a" size=".1"/>
</body>
</worldbody>
</mujoco>
)";
static constexpr char xml_two_geoms[] = R"(
<mujoco>
<worldbody>
<body name="body">
<geom name="a" size=".1"/>
<geom name="b" size=".1" pos="1 0 0"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> er;
mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
ASSERT_THAT(spec, NotNull()) << er.data();
mjModel* m1 = mj_compile(spec, nullptr);
ASSERT_THAT(m1, NotNull());
EXPECT_EQ(m1->nbvh, 1);
// add a geom: the root is no longer a leaf
mjsGeom* geom = mjs_addGeom(mjs_findBody(spec, "body"), nullptr);
mjs_setName(geom->element, "b");
geom->size[0] = .1;
geom->pos[0] = 1;
mjModel* m2 = mj_compile(spec, nullptr);
ASSERT_THAT(m2, NotNull()) << mjs_getError(spec);
MjModelPtr expected =
LoadModelFromString(xml_two_geoms, er.data(), er.size());
ASSERT_THAT(expected.get(), NotNull()) << er.data();
ASSERT_EQ(m2->nbvh, expected->nbvh);
for (int i = 0; i < m2->nbvh; i++) {
EXPECT_EQ(m2->bvh_nodeid[i], expected->bvh_nodeid[i]) << "node " << i;
}
// delete both geoms: no hierarchy is left
EXPECT_EQ(mjs_delete(spec, mjs_findElement(spec, mjOBJ_GEOM, "a")), 0);
EXPECT_EQ(mjs_delete(spec, mjs_findElement(spec, mjOBJ_GEOM, "b")), 0);
mjModel* m3 = mj_compile(spec, nullptr);
ASSERT_THAT(m3, NotNull()) << mjs_getError(spec);
EXPECT_EQ(m3->nbvh, 0);
EXPECT_EQ(m3->body_bvhnum[1], 0);
mj_deleteModel(m1);
mj_deleteModel(m2);
mj_deleteModel(m3);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, RecompileEdit) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<freejoint/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1000> er;
mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
EXPECT_THAT(spec, NotNull()) << er.data();
mjModel* m1 = mj_compile(spec, nullptr);
EXPECT_THAT(m1, NotNull());
// add a geom
mjsBody* world = mjs_findBody(spec, "world");
mjsGeom* geom = mjs_addGeom(world, nullptr);
geom->size[0] = 1;
// compile again
mjModel* m2 = mj_compile(spec, nullptr);
EXPECT_THAT(m2, NotNull());
mj_deleteModel(m1);
mj_deleteModel(m2);
mj_deleteSpec(spec);
}
// editing a frame's pose after a compile takes effect in the next compile
TEST_F(MujocoTest, RecompileEditFrame) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<frame name="outer" pos="0 0 1">
<frame name="inner" pos="1 0 0">
<geom name="geom" size=".1"/>
<body name="body">
<freejoint/>
<geom size=".1"/>
</body>
</frame>
</frame>
</worldbody>
</mujoco>
)";
std::array<char, 1000> er;
mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
ASSERT_THAT(spec, NotNull()) << er.data();
mjModel* m1 = mj_compile(spec, nullptr);
ASSERT_THAT(m1, NotNull());
int geom = mj_name2id(m1, mjOBJ_GEOM, "geom");
int body = mj_name2id(m1, mjOBJ_BODY, "body");
const mjtNum tol = MjTol(1e-12, 1e-6);
const mjtNum pos1[3] = {1, 0, 1};
for (int i = 0; i < 3; i++) {
EXPECT_NEAR(m1->geom_pos[3 * geom + i], pos1[i], tol);
EXPECT_NEAR(m1->body_pos[3 * body + i], pos1[i], tol);
}
// move the outer frame; move the inner one and rotate it 90 degrees about z
mjsFrame* outer = mjs_findFrame(spec, "outer");
mjsFrame* inner = mjs_findFrame(spec, "inner");
outer->pos[2] = 5;
inner->pos[0] = 0;
inner->pos[1] = 2;
inner->quat[0] = inner->quat[3] = mju_sqrt(0.5);
mjModel* m2 = mj_compile(spec, nullptr);
ASSERT_THAT(m2, NotNull());
const mjtNum pos2[3] = {0, 2, 5};
const mjtNum quat2[4] = {mju_sqrt(0.5), 0, 0, mju_sqrt(0.5)};
for (int i = 0; i < 3; i++) {
EXPECT_NEAR(m2->geom_pos[3 * geom + i], pos2[i], tol);
EXPECT_NEAR(m2->body_pos[3 * body + i], pos2[i], tol);
EXPECT_NEAR(m2->qpos0[i], pos2[i], tol);
}
for (int i = 0; i < 4; i++) {
EXPECT_NEAR(m2->body_quat[4 * body + i], quat2[i], tol);
}
mj_deleteModel(m1);
mj_deleteModel(m2);
mj_deleteSpec(spec);
}
// ------------------- test cache with modified assets -------------------------
TEST_F(MujocoTest, RecompileCompareObjCache) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh file="cube.obj"/>
</asset>
<worldbody>
<geom type="mesh" mesh="cube"/>
</worldbody>
</mujoco>)";
static constexpr char cube1[] = R"(
v -0.500000 -0.500000 0.500000
v 0.500000 -0.500000 0.500000
v -0.500000 0.500000 0.500000
v 0.500000 0.500000 0.500000
v -0.500000 0.500000 -0.500000
v 0.500000 0.500000 -0.500000
v -0.500000 -0.500000 -0.500000
v 0.500000 -0.500000 -0.500000)";
static constexpr char cube2[] = R"(
v -1 -1 1
v 1 -1 1
v -1 1 1
v 1 1 1
v -1 1 -1
v 1 1 -1
v -1 -1 -1
v 1 -1 -1)";
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
mj_addBufferVFS(vfs.get(), "cube.obj", cube1, sizeof(cube1));
std::array<char, 1024> error;
// load model once
MjModelPtr m =
LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_EQ(m->mesh_vert[0], -0.5);
// update cube.obj, load again
mj_deleteFileVFS(vfs.get(), "cube.obj");
mj_addBufferVFS(vfs.get(), "cube.obj", cube2, sizeof(cube2));
m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_EQ(m->mesh_vert[0], -1);
mj_deleteVFS(vfs.get());
}
// tiny RGB 2 x 3 PNG file
static constexpr uint8_t tex1[] = {
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};
// previous PNG file, but rotated by 180 degrees
static constexpr uint8_t tex2[] = {
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,
0x10, 0x13, 0x13, 0xfb, 0xff, 0xff, 0x3f, 0xc3, 0x7f, 0x06, 0x96,
0xd8, 0xd8, 0x58, 0x46, 0x46, 0x86, 0x17, 0x0c, 0x0c, 0x00, 0x49,
0x22, 0x06, 0x44, 0xe4, 0x91, 0xb8, 0x83, 0x00, 0x00, 0x00, 0x00,
0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, 0x82};
TEST_F(MujocoTest, RecompileComparePngCache) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture content_type="image/png" file="tex.png" type="2d"/>
<material name="material" texture="tex"/>
</asset>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</worldbody>
</mujoco>
)";
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
mj_addBufferVFS(vfs.get(), "tex.png", tex1, sizeof(tex1));
std::array<char, 1024> error;
// load model once
MjModelPtr m =
LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_EQ(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3
mjtByte byte = m->tex_data[0];
// update tex.png, load again
mj_deleteFileVFS(vfs.get(), "tex.png");
mj_addBufferVFS(vfs.get(), "tex.png", tex2, sizeof(tex2));
m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_NE(m->tex_data[0], byte);
EXPECT_EQ(m->tex_data[15], byte); // first pixel is now last pixel
mj_deleteVFS(vfs.get());
}
TEST_F(MujocoTest, DisableCache) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture content_type="image/png" file="tex.png" type="2d"/>
<material name="material" texture="tex"/>
</asset>
<worldbody>
<geom type="plane" material="material" size="4 4 4"/>
</worldbody>
</mujoco>
)";
mjCache* cache = mj_getCache();
std::size_t capacity = mj_getCacheCapacity(cache);
mj_setCacheCapacity(cache, 0);
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
mj_addBufferVFS(vfs.get(), "tex.png", tex1, sizeof(tex1));
std::array<char, 1024> error;
// load model once
MjModelPtr m =
LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_EQ(mj_getCacheSize(cache), 0);
EXPECT_EQ(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3
mj_setCacheCapacity(cache, capacity);
mj_deleteVFS(vfs.get());
}
// -------------------------------- test textures ------------------------------
TEST_F(MujocoTest, TextureFromBuffer) {
mjSpec* spec = mj_makeSpec();
mjsTexture* t1 = mjs_addTexture(spec);
mjs_setName(t1->element, "tex1");
t1->type = mjTEXTURE_2D;
t1->width = 3;
t1->height = 2;
t1->nchannel = 3;
mjs_setBuffer(t1->data, (std::byte*)tex1, 18);
mjsTexture* t2 = mjs_addTexture(spec);
mjs_setName(t2->element, "tex2");
t2->type = mjTEXTURE_2D;
t2->width = 3;
t2->height = 2;
t2->nchannel = 3;
mjs_setBuffer(t2->data, (std::byte*)tex2, 18);
mjsMaterial* mat = mjs_addMaterial(spec, nullptr);
mjs_setName(mat->element, "mat");
mjs_setInStringVec(mat->textures, mjTEXROLE_RGB, "tex1");
mjs_setInStringVec(mat->textures, mjTEXROLE_ORM, "tex2");
mjsGeom* geom = mjs_addGeom(mjs_findBody(spec, "world"), nullptr);
mjs_setString(geom->material, "mat");
geom->size[0] = 1;
mjModel* m = mj_compile(spec, nullptr);
EXPECT_THAT(m, NotNull());
EXPECT_EQ(m->ntex, 2);
std::array<char, 1024> err;
std::array<char, 1024> str;
mj_saveXMLString(spec, str.data(), str.size(), err.data(), err.size());
EXPECT_STREQ(err.data(), "XML Error: no support for buffer textures.");
mj_deleteModel(m);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, TestTextureFlip) {
mjSpec* spec = mj_makeSpec();
EXPECT_THAT(spec, NotNull());
std::vector<std::byte> texture_data = {std::byte{1}, std::byte{2},
std::byte{3}, std::byte{4}};
mjsTexture* texture = mjs_addTexture(spec);
mjs_setName(texture->element, "hflipped");
texture->type = mjTEXTURE_2D;
texture->width = 2;
texture->height = 2;
texture->nchannel = 1;
texture->hflip = true;
mjs_setBuffer(texture->data, texture_data.data(), texture_data.size());
texture_data = {std::byte{11}, std::byte{12}, std::byte{13}, std::byte{21},
std::byte{22}, std::byte{23}, std::byte{31}, std::byte{32},
std::byte{33}, std::byte{41}, std::byte{42}, std::byte{43}};
texture = mjs_addTexture(spec);
mjs_setName(texture->element, "vflipped");
texture->type = mjTEXTURE_2D;
texture->width = 2;
texture->height = 2;
texture->nchannel = 3;
texture->vflip = true;
mjs_setBuffer(texture->data, texture_data.data(), texture_data.size());
mjModel* model = mj_compile(spec, 0);
EXPECT_THAT(model, NotNull()) << mjs_getError(spec);
EXPECT_THAT(model->tex_data[0], 2);
EXPECT_THAT(model->tex_data[1], 1);
EXPECT_THAT(model->tex_data[2], 4);
EXPECT_THAT(model->tex_data[3], 3);
EXPECT_THAT(model->tex_data[4], 31);
EXPECT_THAT(model->tex_data[5], 32);
EXPECT_THAT(model->tex_data[6], 33);
EXPECT_THAT(model->tex_data[7], 41);
EXPECT_THAT(model->tex_data[8], 42);
EXPECT_THAT(model->tex_data[9], 43);
EXPECT_THAT(model->tex_data[10], 11);
EXPECT_THAT(model->tex_data[11], 12);
EXPECT_THAT(model->tex_data[12], 13);
EXPECT_THAT(model->tex_data[13], 21);
EXPECT_THAT(model->tex_data[14], 22);
EXPECT_THAT(model->tex_data[15], 23);
mj_deleteModel(model);
mj_deleteSpec(spec);
}
// -------------------------------- test attach --------------------------------
static constexpr char xml_child[] = R"(
<mujoco model="child">
<default>
<default class="cylinder">
<geom type="cylinder" size=".1 1 0"/>
</default>
</default>
<custom>
<numeric data="10" name="constant"/>
</custom>
<asset>
<texture name="texture" type="2d" builtin="checker" width="32" height="32"/>
<material name="material" texture="texture" texrepeat="1 1" texuniform="true"/>
</asset>
<worldbody>
<frame name="pframe">
<frame name="cframe">
<body name="body">
<joint type="hinge" name="hinge"/>
<geom class="cylinder" material="material"/>
<light mode="targetbody" target="targetbody"/>
<site name="site" material="material"/>
<body name="targetbody"/>
<body/>
</body>
</frame>
</frame>
<body name="ignore">
<geom size=".1"/>
<joint type="slide"/>
</body>
<frame name="frame" pos=".1 0 0" euler="0 90 0"/>
</worldbody>
<sensor>
<framepos name="sensor" objtype="body" objname="body"/>
<framepos name="ignore" objtype="body" objname="ignore"/>
</sensor>
<tendon>
<fixed name="fixed">
<joint joint="hinge" coef="2"/>
</fixed>
</tendon>
<actuator>
<position name="hinge" joint="hinge" timeconst=".01"/>
<position name="fixed" tendon="fixed" timeconst=".01"/>
<position name="site" site="site" timeconst=".01"/>
</actuator>
<contact>
<exclude body1="body" body2="targetbody"/>
</contact>
<keyframe>
<key name="two" time="2" qpos="2 22" act="1 2 3" ctrl="1 2 3"/>
<key name="three" time="3" qpos="3 33" act="4 5 6" ctrl="4 5 6"/>
</keyframe>
</mujoco>)";
TEST_F(MujocoTest, AttachSame) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml_result[] = R"(
<mujoco model="child">
<default>
<default class="cylinder">
<geom type="cylinder" size=".1 1 0"/>
</default>
</default>
<custom>
<numeric data="10" name="constant"/>
</custom>
<asset>
<texture name="texture" type="2d" builtin="checker" width="32" height="32"/>
<material name="material" texture="texture" texrepeat="1 1" texuniform="true"/>
</asset>
<worldbody>
<body name="body">
<joint type="hinge" name="hinge"/>
<geom class="cylinder" material="material"/>
<light mode="targetbody" target="targetbody"/>
<site name="site" material="material"/>
<body name="targetbody"/>
<body/>
</body>
<body name="ignore">
<geom size=".1"/>
<joint type="slide"/>
</body>
<frame name="frame" pos=".1 0 0" euler="0 90 0">
<body name="attached-body-1">
<joint type="hinge" name="attached-hinge-1"/>
<geom class="cylinder" material="material"/>
<light mode="targetbody" target="attached-targetbody-1"/>
<site name="attached-site-1" material="material"/>
<body name="attached-targetbody-1"/>
<body/>
</body>
</frame>
</worldbody>
<sensor>
<framepos name="sensor" objtype="body" objname="body"/>
<framepos name="ignore" objtype="body" objname="ignore"/>
<framepos name="attached-sensor-1" objtype="body" objname="attached-body-1"/>
</sensor>
<tendon>
<fixed name="fixed">
<joint joint="hinge" coef="2"/>
</fixed>
<fixed name="attached-fixed-1">
<joint joint="attached-hinge-1" coef="2"/>
</fixed>
</tendon>
<actuator>
<position name="hinge" joint="hinge" timeconst=".01"/>
<position name="fixed" tendon="fixed" timeconst=".01"/>
<position name="site" site="site" timeconst=".01"/>
<position name="attached-hinge-1" joint="attached-hinge-1" timeconst=".01"/>
<position name="attached-fixed-1" tendon="attached-fixed-1" timeconst=".01"/>
<position name="attached-site-1" site="attached-site-1" timeconst=".01"/>
</actuator>
<contact>
<exclude body1="body" body2="targetbody"/>
<exclude body1="attached-body-1" body2="attached-targetbody-1"/>
</contact>
<keyframe>
<key name="two" time="2" qpos="2 22 0" act="1 2 3 0 0 0" ctrl="1 2 3 0 0 0"/>
<key name="three" time="3" qpos="3 33 0" act="4 5 6 0 0 0" ctrl="4 5 6 0 0 0"/>
<key name="attached-two-1" time="2" qpos="0 22 2" act="0 0 0 1 2 3" ctrl="0 0 0 1 2 3"/>
<key name="attached-three-1" time="3" qpos="0 33 3" act="0 0 0 4 5 6" ctrl="0 0 0 4 5 6"/>
</keyframe>
</mujoco>)";
// create parent
mjSpec* parent = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjs_setDeepCopy(parent, true); // needed for self-attach
// get frame
mjsFrame* frame = mjs_findFrame(parent, "frame");
EXPECT_THAT(frame, NotNull());
// get subtree
mjsBody* body = mjs_findBody(parent, "body");
EXPECT_THAT(body, NotNull());
// attach child to parent frame
mjsBody* attached =
mjs_asBody(mjs_attach(frame->element, body->element, "attached-", "-1"));
EXPECT_THAT(attached, mjs_findBody(parent, "attached-body-1"));
// check that the spec was not copied
EXPECT_THAT(mjs_findSpec(parent, "child"), IsNull());
// compile new model
mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull());
// check full name stored in mjModel
EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 5), "attached-body-1");
// check body 3 is attached to the world
EXPECT_THAT(m_attached->body_parentid[4], 0);
// compare with expected XML
MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
;
// destroy everything
mj_deleteSpec(parent);
mj_deleteModel(m_attached);
}
TEST_F(MujocoTest, AttachSpatialTendonWithoutSidesite) {
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody>
<body name="parent_body">
<geom size="0.1" type="sphere"/>
</body>
</worldbody>
</mujoco>)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="child_body">
<geom name="wrap_geom" size="0.05" type="sphere"/>
<site name="site_A" pos="0 0 0.1"/>
<site name="site_B" pos="0 0 -0.1"/>
<site name="side_site" pos="0.05 0 0"/>
</body>
</worldbody>
<tendon>
<spatial name="tendon_with_sidesite">
<site site="site_A"/>
<geom geom="wrap_geom" sidesite="side_site"/>
<site site="site_B"/>
</spatial>
<spatial name="tendon_without_sidesite">
<site site="site_A"/>
<geom geom="wrap_geom"/>
<site site="site_B"/>
</spatial>
</tendon>
</mujoco>)";
std::array<char, 1000> er;
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
ASSERT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
ASSERT_THAT(child, NotNull()) << er.data();
mjsBody* parent_body = mjs_findBody(parent, "parent_body");
ASSERT_THAT(parent_body, NotNull());
mjsSite* attach_site = mjs_addSite(parent_body, 0);
mjs_setName(attach_site->element, "attach_site");
mjs_attach(attach_site->element, mjs_findBody(child, "child_body")->element,
"", "_child");
EXPECT_THAT(
mjs_findElement(parent, mjOBJ_TENDON, "tendon_with_sidesite_child"),
NotNull());
EXPECT_THAT(
mjs_findElement(parent, mjOBJ_TENDON, "tendon_without_sidesite_child"),
NotNull());
mjsTendon* tendon = mjs_asTendon(
mjs_findElement(parent, mjOBJ_TENDON, "tendon_with_sidesite_child"));
ASSERT_THAT(tendon, NotNull());
ASSERT_EQ(mjs_getWrapNum(tendon), 3);
EXPECT_EQ(mjs_getWrapTarget(mjs_getWrap(tendon, 0)),
mjs_findElement(parent, mjOBJ_SITE, "site_A_child"));
EXPECT_EQ(mjs_getWrapTarget(mjs_getWrap(tendon, 1)),
mjs_findElement(parent, mjOBJ_GEOM, "wrap_geom_child"));
EXPECT_EQ(mjs_getWrapTarget(mjs_getWrap(tendon, 2)),
mjs_findElement(parent, mjOBJ_SITE, "site_B_child"));
EXPECT_EQ(mjs_getWrapSideSite(mjs_getWrap(tendon, 1)),
mjs_asSite(mjs_findElement(parent, mjOBJ_SITE, "side_site_child")));
mjModel* model = mj_compile(parent, nullptr);
ASSERT_THAT(model, NotNull()) << mjs_getError(parent);
EXPECT_EQ(model->ntendon, 2);
mj_deleteModel(model);
mj_deleteSpec(parent);
mj_deleteSpec(child);
}
TEST_F(MujocoTest, AttachSpatialTendonGitHubIssue3119) {
static constexpr char parent_xml[] = R"(
<mujoco>
<worldbody>
<body name="parent_body">
<geom size="0.1" type="sphere"/>
</body>
</worldbody>
</mujoco>)";
static constexpr char child_xml[] = R"(
<mujoco>
<worldbody>
<body name="child_body">
<geom name="wrap_geom" size="0.05" type="sphere"/>
<site name="site_A" pos="0 0 0.1"/>
<site name="site_B" pos="0 0 -0.1"/>
<site name="side_site" pos="0.05 0 0"/>
</body>
</worldbody>
<tendon>
<spatial name="tendon_with_sidesite">
<site site="site_A"/>
<geom geom="wrap_geom" sidesite="side_site"/>
<site site="site_B"/>
</spatial>
<spatial name="tendon_without_sidesite">
<site site="site_A"/>
<geom geom="wrap_geom"/>
<site site="site_B"/>
</spatial>
</tendon>
</mujoco>)";
std::array<char, 1000> er;
mjSpec* parent_spec = mj_parseXMLString(parent_xml, 0, er.data(), er.size());
ASSERT_THAT(parent_spec, NotNull()) << er.data();
mjSpec* child_spec = mj_parseXMLString(child_xml, 0, er.data(), er.size());
ASSERT_THAT(child_spec, NotNull()) << er.data();
mjsBody* parent_body = mjs_findBody(parent_spec, "parent_body");
ASSERT_THAT(parent_body, NotNull());
mjsSite* attach_site = mjs_addSite(parent_body, 0);
mjs_setName(attach_site->element, "attach_site");
mjs_attach(attach_site->element,
mjs_findBody(child_spec, "child_body")->element, "", "_child");
EXPECT_THAT(
mjs_findElement(parent_spec, mjOBJ_TENDON, "tendon_with_sidesite_child"),
NotNull());
EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON,
"tendon_without_sidesite_child"),
NotNull());
mjModel* model = mj_compile(parent_spec, nullptr);
ASSERT_THAT(model, NotNull()) << mjs_getError(parent_spec);
EXPECT_EQ(model->ntendon, 2);
mj_deleteModel(model);
mj_deleteSpec(parent_spec);
mj_deleteSpec(child_spec);
}
TEST_F(MujocoTest, AttachDifferent) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml_parent[] = R"(
<mujoco model="parent">
<default>
<default class="geom_size">
<geom size="0.1"/>
</default>
</default>
<worldbody>
<body name="sphere">
<freejoint/>
<geom class="geom_size"/>
<frame name="frame" pos=".1 0 0" euler="0 90 0"/>
</body>
</worldbody>
<keyframe>
<key name="one" time="1" qpos="1 1 1 1 0 0 0"/>
</keyframe>
</mujoco>)";
static constexpr char xml_result[] = R"(
<mujoco model="parent">
<default>
<default class="geom_size">
<geom size="0.1"/>
</default>
<default class="attached-cylinder-1">
<geom type="cylinder" size=".1 1 0"/>
</default>
</default>
<custom>
<numeric data="10" name="attached-constant-1"/>
</custom>
<asset>
<texture name="attached-texture-1" type="2d" builtin="checker" width="32" height="32"/>
<material name="attached-material-1" texture="attached-texture-1" texrepeat="1 1" texuniform="true"/>
</asset>
<worldbody>
<body name="sphere">
<freejoint/>
<geom class="geom_size"/>
<frame name="frame" pos=".1 0 0" euler="0 90 0">
<body name="attached-body-1">
<joint type="hinge" name="attached-hinge-1"/>
<geom class="attached-cylinder-1" material="attached-material-1"/>
<light mode="targetbody" target="attached-targetbody-1"/>
<site name="attached-site-1" material="attached-material-1"/>
<body name="attached-targetbody-1"/>
<body/>
</body>
</frame>
</body>
</worldbody>
<sensor>
<framepos name="attached-sensor-1" objtype="body" objname="attached-body-1"/>
</sensor>
<tendon>
<fixed name="attached-fixed-1">
<joint joint="attached-hinge-1" coef="2"/>
</fixed>
</tendon>
<actuator>
<position name="attached-hinge-1" joint="attached-hinge-1" timeconst=".01"/>
<position name="attached-fixed-1" tendon="attached-fixed-1" timeconst=".01"/>
<position name="attached-site-1" site="attached-site-1" timeconst=".01"/>
</actuator>
<contact>
<exclude body1="attached-body-1" body2="attached-targetbody-1"/>
</contact>
<keyframe>
<key name="one" time="1" qpos="1 1 1 1 0 0 0 0"/>
<key name="attached-two-1" time="2" qpos="0 0 0 1 0 0 0 2" act="1 2 3" ctrl="1 2 3"/>
<key name="attached-three-1" time="3" qpos="0 0 0 1 0 0 0 3" act="4 5 6" ctrl="4 5 6"/>
</keyframe>
</mujoco>)";
// model with one free sphere and a frame
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
// get frame
mjsFrame* frame = mjs_findFrame(parent, "frame");
EXPECT_THAT(frame, NotNull());
// model with one cylinder and a hinge
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
// get subtree
mjsBody* body = mjs_findBody(child, "body");
EXPECT_THAT(body, NotNull());
// attach child to parent frame
mjsBody* attached =
mjs_asBody(mjs_attach(frame->element, body->element, "attached-", "-1"));
EXPECT_THAT(attached, mjs_findBody(parent, "attached-body-1"));
// check that the spec was copied
EXPECT_THAT(mjs_findSpec(parent, "child"), NotNull());
// compile new model
mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull());
// check frame is present
EXPECT_THAT(mjs_findFrame(parent, "frame"), NotNull());
// check full name stored in mjModel
EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 2), "attached-body-1");
// check body 2 is attached to body 1
EXPECT_THAT(m_attached->body_parentid[2], 1);
// check that the correct defaults are present
EXPECT_THAT(mjs_findDefault(parent, "main"), NotNull());
EXPECT_THAT(mjs_findDefault(parent, "geom_size"), NotNull());
EXPECT_THAT(mjs_findDefault(parent, "attached-cylinder-1"), NotNull());
// compare with expected XML
MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
;
// destroy everything
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(m_attached);
}
TEST_F(MujocoTest, AttachFrame) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml_parent[] = R"(
<mujoco model="parent">
<worldbody>
<body name="sphere">
<freejoint/>
<geom size=".1"/>
<frame name="frame" pos=".1 0 0" euler="0 90 0"/>
</body>
</worldbody>
<keyframe>
<key name="one" time="1" qpos="1 1 1 1 0 0 0"/>
</keyframe>
</mujoco>)";
static constexpr char xml_result[] = R"(
<mujoco model="parent">
<default>
<default class="attached-cylinder-1">
<geom type="cylinder" size=".1 1 0"/>
</default>
</default>
<custom>
<numeric data="10" name="attached-constant-1"/>
</custom>
<asset>
<texture name="attached-texture-1" type="2d" builtin="checker" width="32" height="32"/>
<material name="attached-material-1" texture="attached-texture-1" texrepeat="1 1" texuniform="true"/>
</asset>
<worldbody>
<body name="sphere">
<freejoint/>
<geom size=".1"/>
<frame name="frame" pos=".1 0 0" euler="0 90 0"/>
<frame name="pframe">
<frame name="cframe">
<body name="attached-body-1">
<joint type="hinge" name="attached-hinge-1"/>
<geom class="attached-cylinder-1" material="attached-material-1"/>
<light mode="targetbody" target="attached-targetbody-1"/>
<site name="attached-site-1" material="attached-material-1"/>
<body name="attached-targetbody-1"/>
<body/>
</body>
</frame>
</frame>
</body>
</worldbody>
<sensor>
<framepos name="attached-sensor-1" objtype="body" objname="attached-body-1"/>
</sensor>
<tendon>
<fixed name="attached-fixed-1">
<joint joint="attached-hinge-1" coef="2"/>
</fixed>
</tendon>
<actuator>
<position name="attached-hinge-1" joint="attached-hinge-1" timeconst=".01"/>
<position name="attached-fixed-1" tendon="attached-fixed-1" timeconst=".01"/>
<position name="attached-site-1" site="attached-site-1" timeconst=".01"/>
</actuator>
<contact>
<exclude body1="attached-body-1" body2="attached-targetbody-1"/>
</contact>
<keyframe>
<key name="one" time="1" qpos="1 1 1 1 0 0 0 0"/>
<key name="attached-two-1" time="2" qpos="0 0 0 1 0 0 0 2" act="1 2 3" ctrl="1 2 3"/>
<key name="attached-three-1" time="3" qpos="0 0 0 1 0 0 0 3" act="4 5 6" ctrl="4 5 6"/>
</keyframe>
</mujoco>)";
// model with one free sphere and a frame
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
// get body
mjsBody* body = mjs_findBody(parent, "sphere");
EXPECT_THAT(body, NotNull());
// model with one cylinder and a hinge
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
// get subtree
mjsFrame* frame = mjs_findFrame(child, "pframe");
EXPECT_THAT(frame, NotNull());
// attach child frame to parent body
mjsFrame* attached =
mjs_asFrame(mjs_attach(body->element, frame->element, "attached-", "-1"));
EXPECT_THAT(attached, mjs_findFrame(parent, "attached-pframe-1"));
// check that the spec was copied
EXPECT_THAT(mjs_findSpec(parent, "child"), NotNull());
// check that the parent body was not namespaced
EXPECT_THAT(mjs_findBody(child, "world"), NotNull());
// compile new model
mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull());
// check full name stored in mjModel
EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 2), "attached-body-1");
// check body 2 is attached to body 1
EXPECT_THAT(m_attached->body_parentid[2], 1);
// compare with expected XML
MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
;
// destroy everything
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(m_attached);
}
TEST_F(MujocoTest, AttachCompiled) {
std::array<char, 1000> er;
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody>
<geom name="floor" pos="0 0 0" size="0 0 0.05" type="plane"/>
</worldbody>
</mujoco>)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="base">
<freejoint/>
<geom name="geom1" size="0.1" type="sphere"/>
</body>
</worldbody>
</mujoco>)";
// load parent
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
// load child
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
// compile child
mjModel* m_child = mj_compile(child, 0);
EXPECT_THAT(m_child, NotNull()) << mjs_getError(child);
// add frame to the parent to attach
mjsBody* world = mjs_findBody(parent, "world");
EXPECT_THAT(world, NotNull()) << mjs_getError(parent);
mjsFrame* frame = mjs_addFrame(world, 0);
EXPECT_THAT(frame, NotNull()) << mjs_getError(parent);
// attach child body to the frame
mjsBody* to_attach = mjs_findBody(child, "base");
EXPECT_THAT(to_attach, NotNull()) << mjs_getError(child);
mjs_attach(frame->element, to_attach->element, "", "");
// check that attached model can be compiled
mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull())
<< "Failed to compile attached model" << mjs_getError(parent);
// destroy everything
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(m_attached);
mj_deleteModel(m_child);
}
void TestDetachBody(bool compile) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml_result[] = R"(
<mujoco model="child">
<asset>
<texture name="texture" type="2d" builtin="checker" width="32" height="32"/>
<material name="material" texture="texture" texrepeat="1 1" texuniform="true"/>
</asset>
<custom>
<numeric data="10" name="constant"/>
</custom>
<worldbody>
<frame name="pframe">
<frame name="cframe">
</frame>
</frame>
<body name="ignore">
<geom size=".1"/>
<joint type="slide"/>
</body>
<frame name="frame" pos=".1 0 0" euler="0 90 0"/>
</worldbody>
<sensor>
<framepos name="ignore" objtype="body" objname="ignore"/>
</sensor>
<keyframe>
<key name="two" time="2" qpos="22"/>
<key name="three" time="3" qpos="33"/>
</keyframe>
</mujoco>)";
// model with one cylinder and a hinge
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
// compile model (for testing double compilation)
mjModel* m_child = compile ? mj_compile(child, 0) : nullptr;
// get subtree
mjsBody* body = mjs_findBody(child, "body");
EXPECT_THAT(body, NotNull());
// delete subtree
EXPECT_THAT(mjs_delete(child, body->element), 0);
// try saving to XML before compiling again
std::array<char, 1024> e;
std::array<char, 1024> s;
EXPECT_EQ(mj_saveXMLString(child, s.data(), 1024, e.data(), 1024), -1);
EXPECT_THAT(e.data(), compile
? HasSubstr("Model has pending keyframes")
: HasSubstr("Only compiled model can be written"));
// compile new model
mjModel* m_detached = mj_compile(child, 0);
EXPECT_THAT(m_detached, NotNull());
// compare with expected XML
MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(m_detached, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
// destroy everything
mj_deleteSpec(child);
mj_deleteModel(m_detached);
if (m_child) mj_deleteModel(m_child);
}
TEST_F(MujocoTest, DetachBody) {
TestDetachBody(/*compile=*/false);
TestDetachBody(/*compile=*/true);
}
TEST_F(MujocoTest, AttachToSite) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody>
<site name="site" pos="1 0 0" quat="0 1 0 0"/>
</worldbody>
</mujoco>)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="sphere">
<joint type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>)";
static constexpr char xml_result[] = R"(
<mujoco>
<worldbody>
<site name="site" pos="1 0 0" quat="0 1 0 0"/>
<frame pos="1 0 0" quat="0 1 0 0">
<body name="attached-sphere-1">
<joint type="slide"/>
<geom size=".1"/>
</body>
</frame>
</worldbody>
</mujoco>)";
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
mjsBody* world = mjs_findBody(parent, "world");
EXPECT_THAT(world, NotNull());
mjsSite* site = mjs_asSite(mjs_firstChild(world, mjOBJ_SITE, 0));
EXPECT_THAT(site, NotNull());
mjsBody* body = mjs_findBody(child, "sphere");
EXPECT_THAT(body, NotNull());
mjsBody* attached =
mjs_asBody(mjs_attach(site->element, body->element, "attached-", "-1"));
EXPECT_THAT(attached, NotNull());
mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull());
MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model);
}
TEST_F(MujocoTest, AttachFrameToSite) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody>
<site name="site" pos="1 0 0" quat="0 1 0 0"/>
</worldbody>
</mujoco>)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<frame name="frame">
<body name="sphere">
<joint type="slide"/>
<geom size=".1"/>
</body>
</frame>
</worldbody>
</mujoco>)";
static constexpr char xml_result[] = R"(
<mujoco>
<worldbody>
<site name="site" pos="1 0 0" quat="0 1 0 0"/>
<frame pos="1 0 0" quat="0 1 0 0">
<body name="attached-sphere-1">
<joint type="slide"/>
<geom size=".1"/>
</body>
</frame>
</worldbody>
</mujoco>)";
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
mjsBody* world = mjs_findBody(parent, "world");
EXPECT_THAT(world, NotNull());
mjsSite* site = mjs_asSite(mjs_firstChild(world, mjOBJ_SITE, 0));
EXPECT_THAT(site, NotNull());
mjsFrame* frame = mjs_findFrame(child, "frame");
EXPECT_THAT(frame, NotNull());
mjsFrame* attached =
mjs_asFrame(mjs_attach(site->element, frame->element, "attached-", "-1"));
EXPECT_THAT(attached, NotNull());
mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull());
MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model);
}
TEST_F(MujocoTest, BodyToFrame) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody>
<body name="body">
<frame name="frame" pos="1 2 3"/>
</body>
</worldbody>
</mujoco>)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="sphere">
<joint type="slide"/>
<geom size=".1"/>
</body>
<camera pos="0 0 0" quat="1 0 0 0"/>
</worldbody>
</mujoco>)";
static constexpr char xml_result[] = R"(
<mujoco>
<worldbody>
<body name="body">
<frame name="frame" pos="1 2 3">
<body name="attached-sphere-1">
<joint type="slide"/>
<geom size=".1"/>
</body>
<frame name="attached-world-2">
<body name="attached-sphere-2">
<joint type="slide"/>
<geom size=".1"/>
</body>
<camera pos="0 0 0" quat="1 0 0 0"/>
</frame>
</frame>
</body>
</worldbody>
</mujoco>)";
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjSpec* child1 = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child1, NotNull()) << er.data();
mjSpec* child2 = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child2, NotNull()) << er.data();
// attach a body to the frame
mjsFrame* frame = mjs_findFrame(parent, "frame");
EXPECT_THAT(frame, NotNull());
mjsBody* body = mjs_findBody(child1, "sphere");
EXPECT_THAT(body, NotNull());
mjsBody* attached =
mjs_asBody(mjs_attach(frame->element, body->element, "attached-", "-1"));
EXPECT_THAT(attached, NotNull());
mjModel* model1 = mj_compile(parent, 0);
EXPECT_THAT(model1, NotNull());
// attach the world to the same frame and convert it to a frame
mjsBody* world = mjs_findBody(child2, "world");
EXPECT_THAT(world, NotNull());
mjsBody* child_world =
mjs_asBody(mjs_attach(frame->element, world->element, "attached-", "-2"));
EXPECT_THAT(child_world, NotNull());
mjsFrame* frame_world = mjs_bodyToFrame(&child_world);
EXPECT_THAT(frame_world, NotNull());
EXPECT_THAT(child_world, IsNull());
// compile and compare
mjModel* model2 = mj_compile(parent, 0);
EXPECT_THAT(model2, NotNull());
MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model2, expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteSpec(parent);
mj_deleteSpec(child1);
mj_deleteSpec(child2);
mj_deleteModel(model1);
mj_deleteModel(model2);
}
TEST_F(MujocoTest, BodyToFrameWithInertial) {
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<body name="child">
<inertial mass="1" pos="0 0 0" quat="1 0 0 0" diaginertia="1 2 3"/>
</body>
</body>
</worldbody>
</mujoco>)";
std::array<char, 1000> er;
mjSpec* spec = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(spec, NotNull()) << er.data();
mjModel* model = mj_compile(spec, 0);
EXPECT_THAT(model, NotNull());
mjsBody* parent = mjs_findBody(spec, "parent");
EXPECT_THAT(parent, NotNull());
mjsBody* child = mjs_findBody(spec, "child");
EXPECT_THAT(child, NotNull());
mjs_bodyToFrame(&child);
EXPECT_THAT(parent->mass, 1);
EXPECT_THAT(parent->fullinertia[0], 1);
EXPECT_THAT(parent->fullinertia[1], 2);
EXPECT_THAT(parent->fullinertia[2], 3);
mj_deleteSpec(spec);
mj_deleteModel(model);
}
TEST_F(MujocoTest, AttachSpecToSite) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody>
<body name="body">
<site name="site" pos="1 2 3"/>
</body>
</worldbody>
</mujoco>)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="sphere">
<joint type="slide"/>
<geom size=".1"/>
</body>
<camera pos="0 0 0" quat="1 0 0 0"/>
</worldbody>
</mujoco>)";
static constexpr char xml_result[] = R"(
<mujoco>
<worldbody>
<body name="body">
<site name="site" pos="1 2 3"/>
<frame name="attached-world-1" pos="1 2 3">
<body name="attached-sphere-1">
<joint type="slide"/>
<geom size=".1"/>
</body>
<camera pos="0 0 0" quat="1 0 0 0"/>
</frame>
</body>
</worldbody>
</mujoco>)";
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
mjsSite* site = mjs_asSite(mjs_findElement(parent, mjOBJ_SITE, "site"));
EXPECT_THAT(site, NotNull());
// add a frame to the child
mjsBody* world = mjs_findBody(child, "world");
EXPECT_THAT(world, NotNull());
mjsFrame* frame = mjs_addFrame(world, 0);
EXPECT_THAT(frame, NotNull());
mjs_setName(frame->element, "world");
mjs_setFrame(mjs_firstChild(world, mjOBJ_BODY, 0), frame);
mjs_setFrame(mjs_firstChild(world, mjOBJ_CAMERA, 0), frame);
// attach the entire spec to the site
mjsFrame* worldframe =
mjs_asFrame(mjs_attach(site->element, frame->element, "attached-", "-1"));
EXPECT_THAT(worldframe, NotNull());
// compile and compare
mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull());
MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
// check that the child world still exists
mjsBody* child_world = mjs_findBody(child, "world");
EXPECT_THAT(child_world, NotNull());
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model);
}
TEST_F(MujocoTest, AttachSpecToBody) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody>
<body name="body"/>
</worldbody>
</mujoco>)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="sphere">
<joint type="slide"/>
<geom size=".1"/>
</body>
<camera pos="0 0 0" quat="1 0 0 0"/>
</worldbody>
</mujoco>)";
static constexpr char xml_result[] = R"(
<mujoco>
<worldbody>
<body name="body">
<frame name="attached-world-1" pos="1 2 3">
<body name="attached-sphere-1">
<joint type="slide"/>
<geom size=".1"/>
</body>
<camera pos="0 0 0" quat="1 0 0 0"/>
</frame>
</body>
</worldbody>
</mujoco>)";
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
mjsBody* body = mjs_findBody(parent, "body");
EXPECT_THAT(body, NotNull());
// add a frame to the child
mjsBody* world = mjs_findBody(child, "world");
EXPECT_THAT(world, NotNull());
mjsFrame* frame = mjs_addFrame(world, 0);
EXPECT_THAT(frame, NotNull());
mjs_setName(frame->element, "world");
mjs_setFrame(mjs_firstChild(world, mjOBJ_BODY, 0), frame);
mjs_setFrame(mjs_firstChild(world, mjOBJ_CAMERA, 0), frame);
// attach the entire spec to the site
mjsFrame* worldframe =
mjs_asFrame(mjs_attach(body->element, frame->element, "attached-", "-1"));
EXPECT_THAT(worldframe, NotNull());
worldframe->pos[0] = 1;
worldframe->pos[1] = 2;
worldframe->pos[2] = 3;
// compile and compare
mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull());
MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
// check that the child world still exists
mjsBody* child_world = mjs_findBody(child, "world");
EXPECT_THAT(child_world, NotNull());
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model);
}
TEST_F(MujocoTest, PreserveState) {
std::array<char, 1000> er;
std::string field = "";
static constexpr char xml_full[] = R"(
<mujoco>
<worldbody>
<body name="detachable" pos="1 0 0">
<joint type="hinge" axis="0 0 1" name="hinge"/>
<geom type="sphere" size=".1"/>
</body>
<body name="persistent">
<joint type="slide" axis="0 0 1" name="slide"/>
<geom type="sphere" size=".2"/>
</body>
<body name="mocap_detach" mocap="true"/>
<body name="mocap" mocap="true"/>
</worldbody>
<actuator>
<position name="hinge" joint="hinge" timeconst=".01"/>
<position name="slide" joint="slide" timeconst=".01"/>
</actuator>
</mujoco>)";
static constexpr char xml_expected[] = R"(
<mujoco>
<worldbody>
<body name="persistent">
<joint type="slide" axis="0 0 1" name="slide"/>
<geom type="sphere" size=".2"/>
</body>
<body name="newbody" pos="2 0 0">
<joint type="slide" axis="0 0 1"/>
<geom type="sphere" size=".3"/>
</body>
<body name="mocap" mocap="true"/>
</worldbody>
<actuator>
<position name="slide" joint="slide" timeconst=".01"/>
</actuator>
</mujoco>)";
// load spec
mjSpec* spec = mj_parseXMLString(xml_full, 0, er.data(), er.size());
EXPECT_THAT(spec, NotNull()) << er.data();
// compile models
mjModel* model = mj_compile(spec, 0);
EXPECT_THAT(model, NotNull());
MjModelPtr m_expected =
LoadModelFromString(xml_expected, er.data(), er.size());
EXPECT_THAT(m_expected.get(), NotNull());
// create data
mjData* data = mj_makeData(model);
EXPECT_THAT(data, NotNull());
MjDataPtr d_expected = MakeData(m_expected);
EXPECT_THAT(d_expected.get(), NotNull());
// set ctrl
data->ctrl[0] = 1;
data->ctrl[1] = 2;
d_expected.get()->ctrl[0] = 2;
// set mocap
data->mocap_pos[3] = 1;
data->mocap_quat[4] = 0;
data->mocap_quat[5] = 1;
d_expected.get()->mocap_pos[0] = 1;
d_expected.get()->mocap_quat[0] = 0;
d_expected.get()->mocap_quat[1] = 1;
// step models
mj_step(model, data);
mj_step(m_expected.get(), d_expected.get());
EXPECT_THAT(data->time, model->opt.timestep);
// detach subtree
mjsBody* body = mjs_findBody(spec, "detachable");
EXPECT_THAT(body, NotNull());
EXPECT_THAT(mjs_delete(spec, body->element), 0);
// detach mocap
mjsBody* mocap_body = mjs_findBody(spec, "mocap_detach");
EXPECT_THAT(mocap_body, NotNull());
EXPECT_THAT(mjs_delete(spec, mocap_body->element), 0);
// add body
mjsBody* newbody = mjs_addBody(mjs_findBody(spec, "world"), 0);
EXPECT_THAT(newbody, NotNull());
// add geom and joint
mjsGeom* geom = mjs_addGeom(newbody, 0);
mjsJoint* joint = mjs_addJoint(newbody, 0);
// set properties
newbody->pos[0] = 2;
geom->size[0] = .3;
joint->type = mjJNT_SLIDE;
joint->axis[0] = 0;
joint->axis[1] = 0;
joint->axis[2] = 1;
joint->ref = d_expected.get()->qpos[m_expected->nq - 1];
// compile new model
mj_recompile(spec, 0, model, data);
EXPECT_THAT(model, NotNull());
EXPECT_THAT(data->time, model->opt.timestep);
// compare qpos
EXPECT_EQ(model->nq, m_expected->nq);
for (int i = 0; i < model->nq; ++i) {
EXPECT_EQ(data->qpos[i], d_expected.get()->qpos[i]) << i;
}
// compare qvel
EXPECT_EQ(model->nv, m_expected->nv);
for (int i = 0; i < model->nv - 1; ++i) {
EXPECT_EQ(data->qvel[i], d_expected.get()->qvel[i]) << i;
}
// second body was added after stepping so qvel should be zero
EXPECT_EQ(data->qvel[model->nv - 1], 0);
// compare act
EXPECT_EQ(model->na, m_expected->na);
for (int i = 0; i < model->na; ++i) {
EXPECT_EQ(data->act[i], d_expected.get()->act[i]) << i;
}
// compare mocap
EXPECT_EQ(model->nmocap, m_expected->nmocap);
for (int i = 0; i < model->nmocap; ++i) {
for (int j = 0; j < 3; ++j) {
EXPECT_EQ(data->mocap_pos[3 * i + j],
d_expected.get()->mocap_pos[3 * i + j])
<< i;
}
for (int j = 0; j < 4; ++j) {
EXPECT_EQ(data->mocap_quat[4 * i + j],
d_expected.get()->mocap_quat[4 * i + j])
<< i;
}
}
// check that the function is callable with no data
mj_deleteData(data);
mj_recompile(spec, 0, model, nullptr);
// destroy everything
mj_deleteSpec(spec);
mj_deleteModel(model);
}
TEST_F(MujocoTest, RecompileAttach) {
std::array<char, 1000> er;
std::string field = "";
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="body">
<joint type="slide" axis="0 0 1"/>
<geom size=".2"/>
</body>
</worldbody>
</mujoco>)";
mjSpec* parent = mj_makeSpec();
EXPECT_THAT(parent, NotNull());
mjSpec* child1 = mj_parseXMLString(xml, 0, er.data(), er.size());
EXPECT_THAT(child1, NotNull());
mjSpec* child2 = mj_parseXMLString(xml, 0, er.data(), er.size());
EXPECT_THAT(child2, NotNull());
mjsElement* frame1 = mjs_addFrame(mjs_findBody(parent, "world"), 0)->element;
mjs_attach(frame1, mjs_findBody(child1, "body")->element, "child-", "-1");
mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model->nq, 1);
mjData* data = mj_makeData(model);
EXPECT_THAT(data, NotNull());
for (int i = 0; i < 100; i++) {
mj_step(model, data);
}
mjsElement* frame2 = mjs_addFrame(mjs_findBody(parent, "world"), 0)->element;
mjs_attach(frame2, mjs_findBody(child2, "body")->element, "child-", "-2");
EXPECT_EQ(mj_recompile(parent, 0, model, data), 0);
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model->nq, 2);
EXPECT_NE(data->qpos[0], data->qpos[1]);
EXPECT_EQ(data->qpos[1], 0);
mj_deleteData(data);
mj_deleteModel(model);
mj_deleteSpec(child1);
mj_deleteSpec(child2);
mj_deleteSpec(parent);
}
TEST_F(MujocoTest, RecompileControlBlocks) {
std::array<char, 1000> er;
// 1. Deleting first actuator preserves retained actuator's control
static constexpr char xml_delete[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="j1" type="slide"/>
<geom size="0.1"/>
</body>
<body>
<joint name="j2" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<actuator>
<motor name="a1" joint="j1"/>
<motor name="a2" joint="j2"/>
</actuator>
</mujoco>)";
mjSpec* s1 = mj_parseXMLString(xml_delete, 0, er.data(), er.size());
ASSERT_THAT(s1, NotNull()) << er.data();
mjModel* m1 = mj_compile(s1, 0);
ASSERT_THAT(m1, NotNull());
mjData* d1 = mj_makeData(m1);
d1->ctrl[0] = 11.0;
d1->ctrl[1] = 22.0;
mjsElement* a1 = mjs_findElement(s1, mjOBJ_ACTUATOR, "a1");
ASSERT_THAT(a1, NotNull());
EXPECT_EQ(mjs_delete(s1, a1), 0);
EXPECT_EQ(mj_recompile(s1, 0, m1, d1), 0);
EXPECT_EQ(m1->nu, 1);
EXPECT_MJTNUM_EQ(d1->ctrl[0], 22.0);
mj_deleteData(d1);
mj_deleteModel(m1);
mj_deleteSpec(s1);
// 2. Multi-input PID actuator preserves all control slots
static constexpr char xml_pid[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="j" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<actuator>
<pid name="pid" joint="j" kp="10" kv="1" input="pos vel ff"/>
</actuator>
</mujoco>)";
mjSpec* s2 = mj_parseXMLString(xml_pid, 0, er.data(), er.size());
ASSERT_THAT(s2, NotNull()) << er.data();
mjModel* m2 = mj_compile(s2, 0);
ASSERT_THAT(m2, NotNull());
ASSERT_EQ(m2->nu, 3);
mjData* d2 = mj_makeData(m2);
d2->ctrl[0] = 1.0;
d2->ctrl[1] = 2.0;
d2->ctrl[2] = 5.0;
EXPECT_EQ(mj_recompile(s2, 0, m2, d2), 0);
ASSERT_EQ(m2->nu, 3);
EXPECT_MJTNUM_EQ(d2->ctrl[0], 1.0);
EXPECT_MJTNUM_EQ(d2->ctrl[1], 2.0);
EXPECT_MJTNUM_EQ(d2->ctrl[2], 5.0);
mj_deleteData(d2);
mj_deleteModel(m2);
mj_deleteSpec(s2);
// 3. Zero-input actuator followed by scalar actuator (nactuator=2, nu=1)
static constexpr char xml_zero[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="j1" type="slide"/>
<geom size="0.1"/>
</body>
<body>
<joint name="j2" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc0" joint="j1" input="none" motorconst="1" resistance="1"/>
<motor name="m1" joint="j2"/>
</actuator>
</mujoco>)";
mjSpec* s3 = mj_parseXMLString(xml_zero, 0, er.data(), er.size());
ASSERT_THAT(s3, NotNull()) << er.data();
mjModel* m3 = mj_compile(s3, 0);
ASSERT_THAT(m3, NotNull());
ASSERT_EQ(m3->nactuator, 2);
ASSERT_EQ(m3->nu, 1);
mjData* d3 = mj_makeData(m3);
d3->ctrl[0] = 33.0;
EXPECT_EQ(mj_recompile(s3, 0, m3, d3), 0);
ASSERT_EQ(m3->nactuator, 2);
ASSERT_EQ(m3->nu, 1);
EXPECT_MJTNUM_EQ(d3->ctrl[0], 33.0);
mj_deleteData(d3);
mj_deleteModel(m3);
mj_deleteSpec(s3);
// 4. Expanding actuator actdim from 1 to 3 does not read past saved act
static constexpr char xml_actdim[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="j" type="hinge"/>
<geom size="0.1"/>
</body>
</worldbody>
<actuator>
<general name="a" joint="j" dyntype="integrator"/>
</actuator>
</mujoco>)";
mjSpec* s4 = mj_parseXMLString(xml_actdim, 0, er.data(), er.size());
ASSERT_THAT(s4, NotNull()) << er.data();
mjModel* m4 = mj_compile(s4, 0);
ASSERT_THAT(m4, NotNull());
ASSERT_EQ(m4->na, 1);
mjData* d4 = mj_makeData(m4);
d4->act[0] = 42.0;
mjsActuator* act4 = mjs_asActuator(mjs_findElement(s4, mjOBJ_ACTUATOR, "a"));
ASSERT_THAT(act4, NotNull());
act4->dyntype = mjDYN_USER;
act4->actdim = 3;
EXPECT_EQ(mj_recompile(s4, 0, m4, d4), 0);
ASSERT_EQ(m4->na, 3);
EXPECT_MJTNUM_EQ(d4->act[0], 42.0);
EXPECT_MJTNUM_EQ(d4->act[1], 0.0);
EXPECT_MJTNUM_EQ(d4->act[2], 0.0);
mj_deleteData(d4);
mj_deleteModel(m4);
mj_deleteSpec(s4);
}
TEST_F(MujocoTest, RecompileIntegrationState) {
std::array<char, 1000> er;
static constexpr char xml[] = R"(
<mujoco>
<size nuserdata="2"/>
<worldbody>
<body name="b1">
<joint name="j1" type="slide" axis="1 0 0"/>
<geom size="0.1" mass="1"/>
</body>
<body name="b2">
<joint name="j2" type="slide" axis="1 0 0"/>
<geom size="0.1" mass="1"/>
</body>
</worldbody>
<equality>
<joint name="eq1" joint1="j1" active="false"/>
<joint name="eq2" joint1="j2" active="false"/>
</equality>
<actuator>
<motor name="a1" joint="j1" nsample="2" delay="0.01"/>
</actuator>
<sensor>
<jointpos name="s1" joint="j2" nsample="2" delay="0.01"/>
</sensor>
</mujoco>)";
mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
ASSERT_THAT(spec, NotNull()) << er.data();
mjModel* m = mj_compile(spec, 0);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
ASSERT_THAT(d, NotNull());
d->time = 3.25;
d->qpos[0] = 0.4;
d->qpos[1] = -0.2;
d->qvel[0] = 0.3;
d->qvel[1] = -0.1;
d->qfrc_applied[0] = 7.0;
d->qfrc_applied[1] = 9.0;
d->xfrc_applied[6 * 1 + 0] = 8.0;
d->xfrc_applied[6 * 2 + 2] = 12.0;
d->eq_active[0] = 1;
d->eq_active[1] = 1;
d->userdata[0] = 123.0;
d->userdata[1] = 321.0;
d->qacc_warmstart[0] = 456.0;
d->qacc_warmstart[1] = 654.0;
d->ctrl[0] = 4.5;
for (int i = 0; i < m->nhistory; i++) {
d->history[i] = 10.0 + i;
}
int nstate = mj_stateSize(m, mjSTATE_INTEGRATION);
std::vector<mjtNum> state_before(nstate);
mj_getState(m, d, state_before.data(), mjSTATE_INTEGRATION);
// no-op recompile preserves the entire mjSTATE_INTEGRATION
EXPECT_EQ(mj_recompile(spec, 0, m, d), 0);
std::vector<mjtNum> state_after(nstate);
mj_getState(m, d, state_after.data(), mjSTATE_INTEGRATION);
EXPECT_EQ(state_before, state_after);
// delete b1 (along with j1, eq1, a1) and recompile: b2, j2, eq2, s1 state
// preserved
mjsBody* b1 = mjs_findBody(spec, "b1");
ASSERT_THAT(b1, NotNull());
EXPECT_EQ(mjs_delete(spec, b1->element), 0);
EXPECT_EQ(mj_recompile(spec, 0, m, d), 0);
EXPECT_EQ(m->nq, 1);
EXPECT_EQ(m->nv, 1);
EXPECT_EQ(m->nbody, 2);
EXPECT_EQ(m->neq, 1);
EXPECT_MJTNUM_EQ(d->qpos[0], -0.2);
EXPECT_MJTNUM_EQ(d->qvel[0], -0.1);
EXPECT_MJTNUM_EQ(d->qfrc_applied[0], 9.0);
EXPECT_MJTNUM_EQ(d->qacc_warmstart[0], 654.0);
EXPECT_MJTNUM_EQ(d->xfrc_applied[6 * 1 + 2], 12.0);
EXPECT_EQ(d->eq_active[0], 1);
EXPECT_MJTNUM_EQ(d->userdata[0], 123.0);
EXPECT_MJTNUM_EQ(d->userdata[1], 321.0);
ASSERT_EQ(m->nhistory, 6);
for (int i = 0; i < 6; i++) {
EXPECT_MJTNUM_EQ(d->history[i], 16.0 + i);
}
// add a new body/joint/actuator/equality and mutate j2 from slide to ball
mjsBody* b3 = mjs_addBody(mjs_findBody(spec, "world"), 0);
mjsGeom* g3 = mjs_addGeom(b3, 0);
g3->size[0] = 0.1;
g3->mass = 1.0;
mjsJoint* j3 = mjs_addJoint(b3, 0);
mjs_setName(j3->element, "j3");
j3->type = mjJNT_SLIDE;
j3->ref = 0.75;
mjsActuator* a3 = mjs_addActuator(spec, 0);
a3->trntype = mjTRN_JOINT;
mjs_setString(a3->target, "j3");
mjsEquality* eq3 = mjs_addEquality(spec, 0);
eq3->type = mjEQ_JOINT;
eq3->active = 1;
mjs_setString(eq3->name1, "j3");
// delete eq2 and s1 before mutating j2 to ball joint
EXPECT_EQ(mjs_delete(spec, mjs_findElement(spec, mjOBJ_EQUALITY, "eq2")), 0);
EXPECT_EQ(mjs_delete(spec, mjs_findElement(spec, mjOBJ_SENSOR, "s1")), 0);
mjsJoint* j2 = mjs_asJoint(mjs_findElement(spec, mjOBJ_JOINT, "j2"));
ASSERT_THAT(j2, NotNull());
j2->type = mjJNT_BALL;
EXPECT_EQ(mj_recompile(spec, 0, m, d), 0);
ASSERT_EQ(m->nq, 5);
ASSERT_EQ(m->nv, 4);
// j2 changed type from slide to ball: initialized to unit quaternion from
// qpos0 and zero vel
EXPECT_MJTNUM_EQ(d->qpos[0], 1.0);
EXPECT_MJTNUM_EQ(d->qpos[1], 0.0);
EXPECT_MJTNUM_EQ(d->qpos[2], 0.0);
EXPECT_MJTNUM_EQ(d->qpos[3], 0.0);
EXPECT_MJTNUM_EQ(d->qvel[0], 0.0);
EXPECT_MJTNUM_EQ(d->qfrc_applied[0], 0.0);
EXPECT_MJTNUM_EQ(d->qacc_warmstart[0], 0.0);
// newly added j3, a3, eq3 receive defaults while b2 retains xfrc_applied
EXPECT_MJTNUM_EQ(d->qpos[4], 0.75);
EXPECT_MJTNUM_EQ(d->qvel[3], 0.0);
EXPECT_MJTNUM_EQ(d->ctrl[0], 0.0);
EXPECT_EQ(d->eq_active[0], 1);
EXPECT_MJTNUM_EQ(d->xfrc_applied[6 * 1 + 2], 12.0);
EXPECT_MJTNUM_EQ(d->xfrc_applied[6 * 2 + 2], 0.0);
mj_deleteData(d);
mj_deleteModel(m);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, AttachMocap) {
std::array<char, 1000> er;
mjtNum tol = 0;
std::string field = "";
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body pos="1 1 1" quat="0 1 0 0" name="mocap" mocap="true"/>
</worldbody>
<keyframe>
<key name="key" time="1" mpos="2 2 2" mquat="0 0 0 1"/>
</keyframe>
</mujoco>)";
static constexpr char xml_expected[] = R"(
<mujoco>
<worldbody>
<body pos="1 1 1" quat="0 1 0 0" name="mocap" mocap="true"/>
<body pos="1 1 1" quat="0 1 0 0" name="attached-mocap-1" mocap="true"/>
</worldbody>
<keyframe>
<key name="key" time="1" mpos="2 2 2 1 1 1" mquat="0 0 0 1 0 1 0 0"/>
<key name="attached-key-1" time="1" mpos="1 1 1 2 2 2" mquat="0 1 0 0 0 0 0 1"/>
</keyframe>
</mujoco>)";
mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
EXPECT_THAT(spec, NotNull()) << er.data();
mjs_setDeepCopy(spec, true); // needed for self-attach
mjsBody* body = mjs_findBody(spec, "mocap");
EXPECT_THAT(body, NotNull());
mjsBody* world = mjs_findBody(spec, "world");
EXPECT_THAT(world, NotNull());
mjsElement* frame = mjs_addFrame(world, NULL)->element;
mjs_attach(frame, body->element, "attached-", "-1");
mjsBody* attached_body = mjs_findBody(spec, "attached-mocap-1");
EXPECT_THAT(attached_body, NotNull());
mjModel* model = mj_compile(spec, 0);
EXPECT_THAT(model, NotNull());
MjModelPtr m_expected =
LoadModelFromString(xml_expected, er.data(), er.size());
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteSpec(spec);
mj_deleteModel(model);
}
TEST_F(MujocoTest, ReplicateKeyframe) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<replicate count="1" euler="0 0 1.8">
<body name="body" pos="0 -1 0">
<joint type="slide"/>
<geom name="g" size="1"/>
</body>
</replicate>
</worldbody>
<keyframe>
<key name="keyframe" qpos="1"/>
</keyframe>
</mujoco>
)";
std::array<char, 1024> error;
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m.get(), testing::NotNull()) << error.data();
EXPECT_THAT(m->ngeom, 1);
EXPECT_THAT(m->nbody, 2);
// check that the keyframe is resized
EXPECT_THAT(m->nkey, 2);
EXPECT_THAT(m->nq, 1);
EXPECT_THAT(m->key_qpos[0], 1);
EXPECT_THAT(m->key_qpos[1], 0);
EXPECT_STREQ(mj_id2name(m.get(), mjOBJ_KEY, 0), "keyframe0");
EXPECT_STREQ(mj_id2name(m.get(), mjOBJ_KEY, 1), "keyframe");
}
TEST_F(MujocoTest, AttachUnnamedAssets) {
static constexpr char cube[] = R"(
v -1 -1 1
v 1 -1 1
v -1 1 1
v 1 1 1
v -1 1 -1
v 1 1 -1
v -1 -1 -1
v 1 -1 -1)";
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
mj_addBufferVFS(vfs.get(), "cube.obj", cube, sizeof(cube));
mjSpec* child = mj_makeSpec();
mjsMesh* mesh = mjs_addMesh(child, 0);
mjsFrame* frame = mjs_addFrame(mjs_findBody(child, "world"), 0);
mjsGeom* geom = mjs_addGeom(mjs_findBody(child, "world"), 0);
mjs_setFrame(geom->element, frame);
mjs_setString(mesh->file, "cube.obj");
mjs_setString(geom->meshname, "cube");
geom->type = mjGEOM_MESH;
mjSpec* spec = mj_makeSpec();
mjs_attach(mjs_findBody(spec, "world")->element, frame->element, "_", "");
mjModel* model = mj_compile(spec, vfs.get());
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model->nmesh, 1);
EXPECT_STREQ(mj_id2name(model, mjOBJ_MESH, 0), "_cube");
mj_deleteVFS(vfs.get());
mj_deleteSpec(spec);
mj_deleteSpec(child);
mj_deleteModel(model);
}
TEST_F(MujocoTest, InitTexture) {
mjSpec* spec = mj_makeSpec();
EXPECT_THAT(spec, NotNull());
mjsTexture* texture = mjs_addTexture(spec);
mjs_setName(texture->element, "checker");
texture->type = mjTEXTURE_CUBE;
texture->builtin = mjBUILTIN_CHECKER;
texture->width = 300;
texture->height = 300;
mjsMaterial* material = mjs_addMaterial(spec, 0);
mjs_setName(material->element, "floor");
mjs_setInStringVec(material->textures, mjTEXROLE_RGB, "checker");
mjsGeom* floor = mjs_addGeom(mjs_findBody(spec, "world"), 0);
mjs_setString(floor->material, "floor");
floor->type = mjGEOM_PLANE;
floor->size[0] = 1;
floor->size[1] = 1;
floor->size[2] = 0.01;
mjs_setString(floor->material, "floor");
mjModel* model = mj_compile(spec, 0);
EXPECT_THAT(model, NotNull());
mj_deleteModel(model);
mj_deleteSpec(spec);
}
void AttachNestedKeyframe(bool compile) {
static constexpr char parent_xml[] = R"(
<mujoco>
<worldbody>
<body name="body">
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
<frame name="frame"/>
</worldbody>
</mujoco>)";
static constexpr char child_xml[] = R"(
<mujoco>
<worldbody>
<body name="body">
<joint name="joint" type="slide"/>
<geom size=".2"/>
<frame name="frame"/>
</body>
</worldbody>
<keyframe>
<key name="key2" time="2" qpos="2"/>
</keyframe>
</mujoco>)";
static constexpr char gchild_xml[] = R"(
<mujoco>
<worldbody>
<body name="body">
<joint name="joint" type="slide"/>
<geom size=".3"/>
</body>
</worldbody>
<keyframe>
<key name="key1" time="1" qpos="1"/>
</keyframe>
</mujoco>)";
static constexpr char expected_xml[] = R"(
<mujoco>
<worldbody>
<body name="body">
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
<frame name="frame">
<body name="child-body">
<joint name="child-joint" type="slide"/>
<geom size=".2"/>
<frame name="child-frame">
<body name="child-gchild-body">
<joint name="child-gchild-joint" type="slide"/>
<geom size=".3"/>
</body>
</frame>
</body>
</frame>
</worldbody>
<keyframe>
<key name="child-key2" time="2" qpos="0 2 0"/>
<key name="child-gchild-key1" time="1" qpos="0 0 1"/>
</keyframe>
</mujoco>)";
static constexpr char expected_xml_uncompiled[] = R"(
<mujoco>
<worldbody>
<body name="body">
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
<frame name="frame">
<body name="child-body">
<joint name="child-joint" type="slide"/>
<geom size=".2"/>
<frame name="child-frame">
<body name="child-gchild-body">
<joint name="child-gchild-joint" type="slide"/>
<geom size=".3"/>
</body>
</frame>
</body>
</frame>
</worldbody>
<keyframe>
<key name="child-key2" time="2" qpos="0 2 0"/>
<key name="gchild-key1" time="1" qpos="0 0 1"/>
</keyframe>
</mujoco>)";
std::array<char, 1000> er;
mjSpec* parent = mj_parseXMLString(parent_xml, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(child_xml, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
mjSpec* gchild = mj_parseXMLString(gchild_xml, 0, er.data(), er.size());
EXPECT_THAT(gchild, NotNull()) << er.data();
mjs_setDeepCopy(parent, true);
mjs_setDeepCopy(child, true);
// attach gchild to child
mjs_attach(mjs_findFrame(child, "frame")->element,
mjs_findBody(gchild, "body")->element, "gchild-", "");
// compile required before further attachment
mjModel* m_child = compile ? mj_compile(child, 0) : nullptr;
// check warning is issued, empty for a compiled model
MockWarningHandler warning_handler;
if (!compile) {
warning_handler.ExpectWarnings("model has pending keyframes");
}
// attach child to parent
mjs_attach(mjs_findFrame(parent, "frame")->element,
mjs_findBody(child, "body")->element, "child-", "");
if (compile) {
EXPECT_EQ(mjs_numWarnings(parent), 0);
} else {
EXPECT_GE(mjs_numWarnings(parent), 1);
EXPECT_THAT(mjs_getWarning(parent, 0),
HasSubstr("model has pending keyframes"));
}
// compare models
mjtNum tol = 0;
std::string field = "";
mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull());
MjModelPtr m_expected = LoadModelFromString(
compile ? expected_xml : expected_xml_uncompiled, er.data(), er.size());
EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
;
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteSpec(gchild);
mj_deleteModel(m_attached);
mj_deleteModel(m_child);
}
TEST_F(MujocoTest, TestAttachNestedKeyframe) {
mock_warning_handler.ExpectWarnings();
AttachNestedKeyframe(/*compile=*/true);
AttachNestedKeyframe(/*compile=*/false);
}
TEST_F(MujocoTest, RepeatedAttachKeyframe) {
static constexpr char xml_1[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>
<body name="body"/>
</worldbody>
</mujoco>)";
static constexpr char xml_2[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>
<body name="b1">
<joint/>
<geom size="0.1"/>
</body>
<body name="b2">
<joint/>
<geom size="0.1"/>
</body>
</worldbody>
<keyframe>
<key name="home" qpos="1 2" />
</keyframe>
</mujoco>)";
std::array<char, 1000> er;
mjSpec* parent = mj_parseXMLString(xml_1, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(xml_2, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
mjsBody* body_1 = mjs_findBody(parent, "body");
mjsElement* attachment_frame = mjs_addFrame(body_1, 0)->element;
mjs_attach(attachment_frame, mjs_findBody(child, "b1")->element, "b1-", "");
mjModel* model_1 = mj_compile(parent, 0);
EXPECT_THAT(model_1, NotNull());
mjs_attach(attachment_frame, mjs_findBody(child, "b2")->element, "b2-", "");
mjModel* model_2 = mj_compile(parent, 0);
EXPECT_THAT(model_2, NotNull());
EXPECT_EQ(model_1->nkey, 1);
EXPECT_EQ(model_2->nkey, 2);
EXPECT_STREQ(mj_id2name(model_2, mjOBJ_KEY, 0), "b1-home");
EXPECT_STREQ(mj_id2name(model_2, mjOBJ_KEY, 1), "b2-home");
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model_1);
mj_deleteModel(model_2);
}
TEST_F(MujocoTest, ResizeParentKeyframe) {
static constexpr char xml_parent[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>
<frame name="frame"/>
<body name="body">
<joint/>
<geom size="0.1"/>
</body>
</worldbody>
<keyframe>
<key name="home" qpos="1"/>
</keyframe>
</mujoco>)";
static constexpr char xml_child[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>
<body name="body">
<joint/>
<geom size="0.1"/>
</body>
</worldbody>
</mujoco>)";
static constexpr char xml_expected[] = R"(
<mujoco model="MuJoCo Model">
<worldbody>
<body name="body">
<joint/>
<geom size="0.1"/>
</body>
<frame name="frame">
<body name="child-body">
<joint/>
<geom size="0.1"/>
</body>
</frame>
</worldbody>
<keyframe>
<key name="home" qpos="1 0"/>
</keyframe>
</mujoco>)";
std::array<char, 1000> er;
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
EXPECT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
mjs_attach(mjs_findFrame(parent, "frame")->element,
mjs_findBody(child, "body")->element, "child-", "");
mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull());
mjtNum tol = 0;
std::string field = "";
MjModelPtr expected = LoadModelFromString(xml_expected, er.data(), er.size());
EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected.get(), field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model);
}
TEST_F(MujocoTest, KeyframeSizeError) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<replicate count="2" offset="0 1 0">
<body name="ball" pos="0 0 1">
<geom type="sphere" size="0.1"/>
<joint/>
</body>
</replicate>
</worldbody>
<keyframe>
<key name="valid_qpos" qpos="0.5"/>
<key name="invalid_qpos" qpos="0.5 0.25"/>
</keyframe>
</mujoco>
)";
std::array<char, 1000> er;
mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
EXPECT_THAT(spec, IsNull());
EXPECT_THAT(
er.data(),
HasSubstr(
"Keyframe 'invalid_qpos' has invalid qpos size, got 2, should be 1"));
}
TEST_F(MujocoTest, DifferentUnitsAllowed) {
static constexpr char gchild_xml[] = R"(
<mujoco>
<compiler angle="degree"/>
<worldbody>
<body name="gchild" euler="-90 0 0">
<geom type="box" size="1 1 1"/>
<joint name="gchild_joint" range="-180 180"/>
</body>
</worldbody>
</mujoco>
)";
static constexpr char child_xml[] = R"(
<mujoco>
<compiler angle="radian"/>
<worldbody>
<body name="child" euler="-1.5707963 0 0">
<geom type="box" size="1 1 1"/>
<joint name="child_joint" range="-3.1415926 3.1415926"/>
<frame name="frame" euler="1.5707963 0 0"/>
</body>
</worldbody>
</mujoco>
)";
static constexpr char parent_xml[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<geom type="box" size="1 1 1"/>
<joint name="parent_joint" range="-180 180"/>
<frame name="frame" euler="90 0 0"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjSpec* gchild = mj_parseXMLString(gchild_xml, 0, error.data(), error.size());
mjSpec* child = mj_parseXMLString(child_xml, 0, error.data(), error.size());
mjSpec* spec = mj_parseXMLString(parent_xml, 0, error.data(), error.size());
ASSERT_THAT(spec, NotNull()) << error.data();
mjs_attach(mjs_findFrame(child, "frame")->element,
mjs_findBody(gchild, "gchild")->element, "gchild_", "");
mjs_attach(mjs_findFrame(spec, "frame")->element,
mjs_findBody(child, "child")->element, "child_", "");
mjModel* model = mj_compile(spec, 0);
EXPECT_THAT(model, NotNull());
EXPECT_THAT(model->njnt, 3);
EXPECT_NEAR(model->jnt_range[0], -mjPI, 1e-6);
EXPECT_NEAR(model->jnt_range[1], mjPI, 1e-6);
EXPECT_NEAR(model->jnt_range[2], -mjPI, 1e-6);
EXPECT_NEAR(model->jnt_range[3], mjPI, 1e-6);
EXPECT_NEAR(model->jnt_range[4], -mjPI, 1e-6);
EXPECT_NEAR(model->jnt_range[5], mjPI, 1e-6);
EXPECT_NEAR(model->body_quat[4], 1, 1e-12);
EXPECT_NEAR(model->body_quat[5], 0, 1e-12);
EXPECT_NEAR(model->body_quat[6], 0, 1e-12);
EXPECT_NEAR(model->body_quat[7], 0, 1e-12);
mjSpec* copied_spec = mj_copySpec(spec);
ASSERT_THAT(copied_spec, NotNull());
mj_deleteSpec(gchild);
mj_deleteSpec(child);
mj_deleteSpec(spec);
mj_deleteModel(model);
// check that deleting `parent` or `child` does not invalidate the copy
mjModel* copied_model = mj_compile(copied_spec, 0);
EXPECT_THAT(copied_model, NotNull());
EXPECT_THAT(copied_model->njnt, 3);
EXPECT_NEAR(copied_model->jnt_range[0], -mjPI, 1e-6);
EXPECT_NEAR(copied_model->jnt_range[1], mjPI, 1e-6);
EXPECT_NEAR(copied_model->jnt_range[2], -mjPI, 1e-6);
EXPECT_NEAR(copied_model->jnt_range[3], mjPI, 1e-6);
EXPECT_NEAR(copied_model->jnt_range[4], -mjPI, 1e-6);
EXPECT_NEAR(copied_model->jnt_range[5], mjPI, 1e-6);
EXPECT_NEAR(copied_model->body_quat[4], 1, 1e-12);
EXPECT_NEAR(copied_model->body_quat[5], 0, 1e-12);
EXPECT_NEAR(copied_model->body_quat[6], 0, 1e-12);
EXPECT_NEAR(copied_model->body_quat[7], 0, 1e-12);
mj_deleteSpec(copied_spec);
mj_deleteModel(copied_model);
}
TEST_F(MujocoTest, DifferentOptionsInAttachedFrame) {
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody/>
</mujoco>
)";
static constexpr char xml_child[] = R"(
<mujoco>
<compiler eulerseq="zyx"/>
<worldbody>
<frame name="child" >
<site euler="0 90 180"/>
</frame>
</worldbody>
</mujoco>
)";
// load specs and compile child
mjSpec* parent = mj_parseXMLString(xml_parent, 0, nullptr, 0);
EXPECT_THAT(parent, NotNull());
mjSpec* child1 = mj_parseXMLString(xml_child, 0, nullptr, 0);
EXPECT_THAT(child1, NotNull());
mjModel* m_child1 = mj_compile(child1, 0);
EXPECT_THAT(m_child1, NotNull());
mjSpec* child2 = mj_parseXMLString(xml_child, 0, nullptr, 0);
EXPECT_THAT(child2, NotNull());
mjModel* m_child2 = mj_compile(child1, 0);
EXPECT_THAT(m_child2, NotNull());
// attach child frame to parent worldbody
mjsBody* world = mjs_findBody(parent, "world");
EXPECT_THAT(world, NotNull());
mjsFrame* child1_frame = mjs_findFrame(child1, "child");
EXPECT_THAT(child1_frame, NotNull());
mjsFrame* child2_frame = mjs_findFrame(child2, "child");
EXPECT_THAT(child2_frame, NotNull());
mjsElement* attached_frame1 =
mjs_attach(world->element, child1_frame->element, "child-", "-1");
EXPECT_THAT(attached_frame1, NotNull());
mjsElement* attached_frame2 =
mjs_attach(world->element, child2_frame->element, "child-", "-2");
EXPECT_THAT(attached_frame2, NotNull());
// wrap the child frame in the parent frame and compile
mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull());
EXPECT_NEAR(m_attached->site_quat[0], m_child1->site_quat[0], 1e-6);
EXPECT_NEAR(m_attached->site_quat[1], m_child1->site_quat[1], 1e-6);
EXPECT_NEAR(m_attached->site_quat[2], m_child1->site_quat[2], 1e-6);
EXPECT_NEAR(m_attached->site_quat[3], m_child1->site_quat[3], 1e-6);
EXPECT_NEAR(m_attached->site_quat[4], m_child2->site_quat[0], 1e-6);
EXPECT_NEAR(m_attached->site_quat[5], m_child2->site_quat[1], 1e-6);
EXPECT_NEAR(m_attached->site_quat[6], m_child2->site_quat[2], 1e-6);
EXPECT_NEAR(m_attached->site_quat[7], m_child2->site_quat[3], 1e-6);
mj_deleteSpec(parent);
mj_deleteSpec(child1);
mj_deleteModel(m_child1);
mj_deleteSpec(child2);
mj_deleteModel(m_child2);
mj_deleteModel(m_attached);
}
TEST_F(MujocoTest, NotCopyAttachedSpec) {
static constexpr char xml_parent[] = R"(
<mujoco>
<asset>
<model name="child" file="xml_child.xml"/>
</asset>
<worldbody>
<body name="parent">
<geom name="geom" size="2"/>
<attach model="child" body="body" prefix="other"/>
</body>
</worldbody>
</mujoco>
)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="body">
<geom name="geom" size="1" pos="2 0 0"/>
</body>
</worldbody>
</mujoco>
)";
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
mj_addBufferVFS(vfs.get(), "xml_child.xml", xml_child, sizeof(xml_child));
std::array<char, 1024> er;
mjSpec* spec = mj_parseXMLString(xml_parent, vfs.get(), er.data(), er.size());
EXPECT_THAT(spec, NotNull()) << er.data();
mjModel* model = mj_compile(spec, vfs.get());
EXPECT_THAT(model, NotNull()) << er.data();
mjSpec* child = mjs_findSpec(spec, "child");
EXPECT_THAT(child, NotNull());
mjSpec* copy = mj_copySpec(spec);
EXPECT_THAT(copy, NotNull());
mjSpec* child_copy = mjs_findSpec(copy, "child");
EXPECT_THAT(child_copy, NotNull());
EXPECT_EQ(child_copy, child);
mj_deleteSpec(spec);
mj_deleteSpec(copy);
mj_deleteModel(model);
mj_deleteVFS(vfs.get());
}
TEST_F(MujocoTest, ApplyNameSpaceToDefaults) {
static constexpr char xml_c[] = R"(
<mujoco>
<default>
<default class="mesh">
<mesh scale="0.001 0.001 0.001"/>
</default>
</default>
<asset>
<mesh file="cube.obj" class="mesh"/>
</asset>
<worldbody>
<body name="body">
<geom type="mesh" mesh="cube"/>
</body>
</worldbody>
</mujoco>)";
static constexpr char xml_p[] = R"(
<mujoco>
<worldbody>
<frame name="parent"/>
</worldbody>
</mujoco>
)";
static constexpr char cube[] = R"(
v -0.500000 -0.500000 0.500000
v 0.500000 -0.500000 0.500000
v -0.500000 0.500000 0.500000
v 0.500000 0.500000 0.500000
v -0.500000 0.500000 -0.500000
v 0.500000 0.500000 -0.500000
v -0.500000 -0.500000 -0.500000
v 0.500000 -0.500000 -0.500000)";
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
mj_addBufferVFS(vfs.get(), "cube.obj", cube, sizeof(cube));
std::array<char, 1024> err;
mjSpec* child = mj_parseXMLString(xml_c, vfs.get(), err.data(), err.size());
EXPECT_THAT(child, NotNull()) << err.data();
mjSpec* parent = mj_parseXMLString(xml_p, 0, err.data(), err.size());
EXPECT_THAT(parent, NotNull()) << err.data();
mjsElement* attached =
mjs_attach(mjs_findFrame(parent, "parent")->element,
mjs_findBody(child, "body")->element, "child-", "");
EXPECT_THAT(attached, NotNull());
mjModel* model = mj_compile(parent, vfs.get());
EXPECT_THAT(model, NotNull());
mj_deleteSpec(child);
mj_deleteSpec(parent);
mj_deleteModel(model);
mj_deleteVFS(vfs.get());
}
TEST_F(MujocoTest, DetachDefault) {
static constexpr char xml_c[] = R"(
<mujoco>
<default>
<default class="parent">
<default class="child1">
<mesh scale="0.001 0.001 0.001"/>
</default>
<default class="child2">
<mesh scale="0.001 0.001 0.001"/>
</default>
</default>
</default>
<asset>
<mesh file="cube.obj" class="child2"/>
</asset>
<worldbody>
<body name="body">
<geom type="mesh" mesh="cube"/>
</body>
</worldbody>
</mujoco>)";
static constexpr char cube[] = R"(
v -0.500000 -0.500000 0.500000
v 0.500000 -0.500000 0.500000
v -0.500000 0.500000 0.500000
v 0.500000 0.500000 0.500000
v -0.500000 0.500000 -0.500000
v 0.500000 0.500000 -0.500000
v -0.500000 -0.500000 -0.500000
v 0.500000 -0.500000 -0.500000)";
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
mj_addBufferVFS(vfs.get(), "cube.obj", cube, sizeof(cube));
std::array<char, 1024> err;
mjSpec* spec = mj_parseXMLString(xml_c, vfs.get(), err.data(), err.size());
EXPECT_THAT(spec, NotNull()) << err.data();
// get default
mjsDefault* child = mjs_findDefault(spec, "child1");
EXPECT_THAT(child, NotNull());
// delete default
EXPECT_EQ(mjs_delete(spec, child->element), 0);
child = mjs_findDefault(spec, "child1");
EXPECT_THAT(child, IsNull());
// try and detach previously detached default, should fail
EXPECT_EQ(mjs_delete(spec, nullptr), -1);
child = mjs_findDefault(spec, "child1");
EXPECT_THAT(child, IsNull());
// detach parent
mjsDefault* parent = mjs_findDefault(spec, "parent");
EXPECT_THAT(parent, NotNull());
mjs_delete(spec, parent->element);
// both parent and remaining child should be removed
parent = mjs_findDefault(spec, "parent");
EXPECT_THAT(parent, IsNull());
child = mjs_findDefault(spec, "child2");
EXPECT_THAT(child, IsNull());
// error when trying to detach the 'main' default
mjsDefault* main = mjs_findDefault(spec, "main");
EXPECT_THAT(main, NotNull());
EXPECT_EQ(mjs_delete(spec, main->element), -1);
EXPECT_THAT(mjs_getError(spec),
HasSubstr("cannot remove the global default ('main')"));
main = mjs_findDefault(spec, "main");
EXPECT_THAT(main, NotNull());
mj_deleteVFS(vfs.get());
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, ErrorWhenCompilingOrphanedSpec) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="a"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> er;
mjSpec* child = mj_parseXMLString(xml, 0, er.data(), er.size());
EXPECT_THAT(child, NotNull()) << er.data();
mjSpec* parent = mj_makeSpec();
EXPECT_THAT(parent, NotNull());
mjsBody* body = mjs_findBody(child, "a");
EXPECT_THAT(body, NotNull());
mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), nullptr);
EXPECT_THAT(frame, NotNull());
mjs_attach(frame->element, body->element, "child-", "");
mj_deleteSpec(parent);
mjModel* model = mj_compile(child, 0);
EXPECT_THAT(model, IsNull());
EXPECT_THAT(mjs_getError(child), HasSubstr("by reference to a parent"));
mj_deleteSpec(child);
}
TEST_F(MujocoTest, SetFrameReverseOrder) {
mjSpec* spec = mj_makeSpec();
mjsBody* world = mjs_findBody(spec, "world");
mjsFrame* child = mjs_addFrame(world, nullptr);
mjsFrame* parent = mjs_addFrame(world, nullptr);
mjs_setName(child->element, "child");
mjs_setName(parent->element, "parent");
mjs_setFrame(child->element, parent);
mjSpec* copy = mj_copySpec(spec);
EXPECT_THAT(copy, NotNull());
EXPECT_THAT(mjs_findFrame(copy, "child"), NotNull());
EXPECT_THAT(mjs_findFrame(copy, "parent"), NotNull());
mj_deleteSpec(spec);
mj_deleteSpec(copy);
}
TEST_F(MujocoTest, CopyInertialInFrame) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<frame name="frame" pos="1 0 0">
<inertial pos="0 1 0" mass="1" diaginertia="1 1 1"/>
</frame>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
ASSERT_THAT(spec, NotNull()) << error.data();
// move the frame in a copy, the copied inertial follows its own frame
mjSpec* copy = mj_copySpec(spec);
ASSERT_THAT(copy, NotNull());
mjs_findFrame(copy, "frame")->pos[0] = 2;
mjModel* model = mj_compile(copy, 0);
ASSERT_THAT(model, NotNull());
EXPECT_EQ(model->body_ipos[3], 2);
EXPECT_EQ(model->body_ipos[4], 1);
EXPECT_EQ(model->body_ipos[5], 0);
mj_deleteModel(model);
mj_deleteSpec(copy);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, UserValue) {
mjSpec* spec = mj_makeSpec();
EXPECT_THAT(spec, NotNull());
mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), nullptr);
EXPECT_THAT(body, NotNull());
std::string data = "data";
mjs_setUserValue(body->element, "key", data.data());
EXPECT_THAT(mjs_getUserValue(body->element, "invalid_key"), IsNull());
const void* payload = mjs_getUserValue(body->element, "key");
EXPECT_STREQ(static_cast<const char*>(payload), data.c_str());
mjs_deleteUserValue(body->element, "key");
EXPECT_THAT(mjs_getUserValue(body->element, "key"), IsNull());
std::string* heap_data = new std::string("heap_data");
mjs_setUserValueWithCleanup(
body->element, "key", heap_data,
[](const void* data) { delete static_cast<const std::string*>(data); });
payload = mjs_getUserValue(body->element, "key");
EXPECT_STREQ(static_cast<const std::string*>(payload)->c_str(),
heap_data->c_str());
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, CompilerTimers) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture name="grid" type="2d" builtin="checker" width="300" height="300" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
<material name="grid" texture="grid"/>
</asset>
<worldbody>
<geom type="plane" size="1 1 1" material="grid"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
ASSERT_THAT(spec, NotNull()) << error.data();
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull());
EXPECT_GT(mjs_getTimer(spec)[mjCTIMER_TOTAL], 0);
EXPECT_GT(mjs_getTimer(spec)[mjCTIMER_ASSETS], 0);
EXPECT_GT(mjs_getTimer(spec)[mjCTIMER_TEXTURE], 0);
mj_deleteModel(model);
mj_deleteSpec(spec);
}
// -------------------- test compile warning infrastructure --------------------
TEST_F(MujocoTest, CompileWarningCount) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<geom size="1"/>
<flexcomp name="grid" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
dim="2" radius="0.01">
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
ASSERT_THAT(spec, NotNull()) << error.data();
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull());
// flex with no passive forces should produce a warning
EXPECT_GT(mjs_numWarnings(spec), 0);
EXPECT_THAT(mjs_getWarning(spec, 0), HasSubstr("not rigid"));
mj_deleteModel(model);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, CompileWarningOutOfBounds) {
mjSpec* spec = mj_makeSpec();
mjsBody* world = mjs_findBody(spec, "world");
mjsGeom* geom = mjs_addGeom(world, 0);
geom->size[0] = 1;
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull());
// no warnings expected for simple model
EXPECT_EQ(mjs_numWarnings(spec), 0);
EXPECT_THAT(mjs_getWarning(spec, 0), IsNull());
EXPECT_THAT(mjs_getWarning(spec, -1), IsNull());
// nullptr spec should not crash
EXPECT_EQ(mjs_numWarnings(nullptr), 0);
EXPECT_THAT(mjs_getWarning(nullptr, 0), IsNull());
mj_deleteModel(model);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, RecompileClearsCompileWarnings) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<geom size="1"/>
<flexcomp name="grid" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
dim="2" radius="0.01">
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
ASSERT_THAT(spec, NotNull()) << error.data();
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull());
int first_count = mjs_numWarnings(spec);
EXPECT_GT(first_count, 0);
// recompile — warnings should be regenerated, not accumulated
mj_deleteModel(model);
model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull());
EXPECT_EQ(mjs_numWarnings(spec), first_count);
mj_deleteModel(model);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, LoadXMLWarningInErrorBuffer) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<geom size="1"/>
<flexcomp name="grid" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
dim="2" radius="0.01">
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
// write xml to VFS
mjVFS vfs;
mj_defaultVFS(&vfs);
mj_addBufferVFS(&vfs, "model.xml", xml, sizeof(xml));
std::array<char, 1024> error;
error[0] = '\0';
mjModel* model = mj_loadXML("model.xml", &vfs, error.data(), error.size());
ASSERT_THAT(model, NotNull());
// warning should be in the error buffer
EXPECT_THAT(error.data(), HasSubstr("not rigid"));
mj_deleteModel(model);
mj_deleteVFS(&vfs);
}
TEST_F(MujocoTest, CompileWarningChainedToHandler) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="parent">
<geom size="1"/>
<flexcomp name="grid" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
dim="2" radius="0.01">
</flexcomp>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
ASSERT_THAT(spec, NotNull()) << error.data();
// install a custom log handler that captures warnings
std::vector<std::string> captured_warnings;
static thread_local std::vector<std::string>* capture_ptr = nullptr;
capture_ptr = &captured_warnings;
// install custom log handler (replaces global, so mock is bypassed)
mjfLogHandler prev = mju_setLogHandler([](const mjLogMessage* msg) {
if (msg->level == mjLOG_WARNING && capture_ptr) {
capture_ptr->push_back(msg->subject);
}
});
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull());
// restore log handler
mju_setLogHandler(prev);
capture_ptr = nullptr;
// chaining should have forwarded warnings to our handler
EXPECT_THAT(captured_warnings, testing::Contains(HasSubstr("not rigid")));
mj_deleteModel(model);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, WarningAccumulationAndRetrieval) {
mock_warning_handler.ExpectWarnings();
static constexpr char xml_parent[] = R"(
<mujoco>
<worldbody>
<frame name="parent"/>
</worldbody>
</mujoco>
)";
static constexpr char xml_child[] = R"(
<mujoco>
<worldbody>
<body name="child">
<frame name="child_frame"/>
</body>
</worldbody>
</mujoco>
)";
static constexpr char xml_gchild[] = R"(
<mujoco>
<worldbody>
<body name="gchild"/>
</worldbody>
<keyframe>
<key name="k1"/>
</keyframe>
</mujoco>
)";
std::array<char, 1024> er;
mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
ASSERT_THAT(parent, NotNull()) << er.data();
mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
ASSERT_THAT(child, NotNull()) << er.data();
mjSpec* gchild = mj_parseXMLString(xml_gchild, 0, er.data(), er.size());
ASSERT_THAT(gchild, NotNull()) << er.data();
mjs_setDeepCopy(parent, true);
mjs_setDeepCopy(child, true);
mjs_setDeepCopy(gchild, true);
mjs_attach(mjs_findFrame(child, "child_frame")->element,
mjs_findBody(gchild, "gchild")->element, "gchild-", "");
mjs_attach(mjs_findFrame(parent, "parent")->element,
mjs_findBody(child, "child")->element, "child-", "");
EXPECT_EQ(mjs_numWarnings(parent), 1);
EXPECT_THAT(mjs_getWarning(parent, 0),
HasSubstr("Child model has pending keyframes"));
EXPECT_THAT(mjs_getWarning(parent, 1), IsNull());
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteSpec(gchild);
}
TEST_F(MujocoTest, CompilationWarningsClearedOnRecompile) {
mock_warning_handler.ExpectWarnings();
// flex with no edge stiffness or equality triggers passive forces warning
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="grid" count="4 4 1" spacing=".2 .2 .2"
dim="2" radius=".1"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> er;
mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
ASSERT_THAT(spec, NotNull()) << er.data();
// first compile: should generate flex warning
mjModel* m = mj_compile(spec, nullptr);
ASSERT_THAT(m, NotNull());
int n1 = mjs_numWarnings(spec);
EXPECT_EQ(n1, 1);
EXPECT_THAT(mjs_getWarning(spec, 0),
HasSubstr("no equality constraints or passive forces"));
// recompile: warnings should be cleared and regenerated
mj_deleteModel(m);
m = mj_compile(spec, nullptr);
ASSERT_THAT(m, NotNull());
EXPECT_EQ(mjs_numWarnings(spec), n1);
mj_deleteModel(m);
mj_deleteSpec(spec);
}
TEST_F(MujocoTest, MjEncodeNativeFormats) {
// simple test spec
mjSpec* spec = mj_makeSpec();
mjsBody* world = mjs_findBody(spec, "world");
mjsBody* body = mjs_addBody(world, nullptr);
mjsGeom* geom = mjs_addGeom(body, nullptr);
geom->size[0] = 1.0;
geom->size[1] = 1.0;
geom->size[2] = 1.0;
mjModel* model = mj_compile(spec, nullptr);
ASSERT_THAT(model, NotNull());
std::filesystem::path tmp_dir = std::filesystem::temp_directory_path();
std::string xml_path = (tmp_dir / "test_encode.xml").string();
std::string mjb_path = (tmp_dir / "test_encode.mjb").string();
std::string txt_path = (tmp_dir / "test_encode.txt").string();
char error[1000];
// XML Encoding with spec
EXPECT_GT(mj_encode(spec, model, xml_path.c_str(), nullptr, nullptr, error,
sizeof(error)),
0);
EXPECT_TRUE(std::filesystem::exists(xml_path));
EXPECT_GT(std::filesystem::file_size(xml_path), 0);
// XML Encoding without spec (should fail because no XML loaded in global
// spec)
std::filesystem::remove(xml_path);
// free global spec
mj_freeLastXML();
EXPECT_EQ(mj_encode(nullptr, model, xml_path.c_str(), nullptr, nullptr, error,
sizeof(error)),
-1);
EXPECT_THAT(error, HasSubstr("No XML model loaded"));
EXPECT_TRUE(std::filesystem::exists(xml_path));
EXPECT_EQ(std::filesystem::file_size(xml_path), 0);
std::filesystem::remove(xml_path);
// MJB Encoding (spec can be null)
EXPECT_GT(mj_encode(nullptr, model, mjb_path.c_str(), nullptr, nullptr, error,
sizeof(error)),
0);
EXPECT_TRUE(std::filesystem::exists(mjb_path));
EXPECT_GT(std::filesystem::file_size(mjb_path), 0);
mjModel* model2 = mj_loadModel(mjb_path.c_str(), nullptr);
EXPECT_THAT(model2, NotNull());
mj_deleteModel(model2);
// TXT Encoding (spec can be null)
EXPECT_GT(mj_encode(nullptr, model, txt_path.c_str(), nullptr, nullptr, error,
sizeof(error)),
0);
EXPECT_TRUE(std::filesystem::exists(txt_path));
EXPECT_GT(std::filesystem::file_size(txt_path), 0);
// Clean up
std::filesystem::remove(mjb_path);
std::filesystem::remove(txt_path);
mj_deleteModel(model);
mj_deleteSpec(spec);
}
// Tests that joint ordering is preserved after attach operations
TEST_F(MujocoTest, AttachPreservesJointOrder) {
static constexpr char child_xml[] = R"(
<mujoco>
<worldbody>
<body name="slider_body">
<joint name="slide_joint" type="slide"/>
<geom type="box" size="0.1 0.1 0.1"/>
</body>
<body name="free_body" pos="1 0 0">
<freejoint name="free_joint"/>
<geom type="sphere" size="0.1"/>
</body>
</worldbody>
</mujoco>
)";
mjSpec* parent = mj_makeSpec();
mjsBody* parent_world = mjs_findBody(parent, "world");
mjsFrame* attach_frame = mjs_addFrame(parent_world, nullptr);
mjs_setName(attach_frame->element, "attach_frame");
std::array<char, 1000> error;
mjSpec* child =
mj_parseXMLString(child_xml, nullptr, error.data(), error.size());
ASSERT_THAT(child, NotNull()) << error.data();
mjsElement* attached =
mjs_attach(attach_frame->element, child->element, "child_", "");
ASSERT_THAT(attached, NotNull());
mjModel* model = mj_compile(parent, nullptr);
ASSERT_THAT(model, NotNull());
// Verify joint order: slide joint should come before free joint
int slide_id = mj_name2id(model, mjOBJ_JOINT, "child_slide_joint");
int free_id = mj_name2id(model, mjOBJ_JOINT, "child_free_joint");
ASSERT_GE(slide_id, 0);
ASSERT_GE(free_id, 0);
EXPECT_LT(slide_id, free_id)
<< "Slide joint should have a lower ID than free joint";
// Export and re-import to test round-trip preservation
std::string exported_xml = SaveAndReadXml(parent);
mjSpec* reimported = mj_parseXMLString(exported_xml.c_str(), nullptr,
error.data(), error.size());
ASSERT_THAT(reimported, NotNull()) << error.data();
mjModel* reimported_model = mj_compile(reimported, nullptr);
ASSERT_THAT(reimported_model, NotNull());
// Verify joint order is preserved after round-trip
int reimported_slide_id =
mj_name2id(reimported_model, mjOBJ_JOINT, "child_slide_joint");
int reimported_free_id =
mj_name2id(reimported_model, mjOBJ_JOINT, "child_free_joint");
ASSERT_GE(reimported_slide_id, 0);
ASSERT_GE(reimported_free_id, 0);
EXPECT_EQ(reimported_slide_id, slide_id)
<< "Joint IDs should be the same after XML round-trip";
EXPECT_EQ(reimported_free_id, free_id)
<< "Joint IDs should be the same after XML round-trip";
mj_deleteModel(model);
mj_deleteModel(reimported_model);
mj_deleteSpec(child);
mj_deleteSpec(parent);
mj_deleteSpec(reimported);
}
TEST_F(MujocoTest, DeleteReclaimsMemoryImmediately) {
mjSpec* spec = mj_makeSpec();
mjsBody* world = mjs_findBody(spec, "world");
int cleanup_count = 0;
auto cleanup = +[](const void* data) {
*static_cast<int*>(const_cast<void*>(data)) += 1;
};
mjsBody* body = mjs_addBody(world, nullptr);
mjsGeom* geom = mjs_addGeom(body, nullptr);
mjs_setUserValueWithCleanup(body->element, "tracker", &cleanup_count,
cleanup);
mjs_setUserValueWithCleanup(geom->element, "tracker", &cleanup_count,
cleanup);
EXPECT_EQ(cleanup_count, 0);
EXPECT_EQ(mjs_delete(spec, body->element), 0);
EXPECT_EQ(cleanup_count, 2);
mj_deleteSpec(spec);
EXPECT_EQ(cleanup_count, 2);
}
} // namespace
} // namespace mujoco
2717294d7This file is pinned to the selected release. Download the original to open it in your design tool.