chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity
Files
2fa2db7ae0ebc0e55e93e233f668fd6048dc681c
Mujoco_WASM/test/user/user_api_test.cc
T
Sam Haves da01bd37a2 Remove modelfiledir_ from compiled assets, use owning spec modelfiledir instead. 
Previously specs that were attached to some parent spec would resolve its asset paths relative to the modelfiledir of the parent spec. This means path resolution would change depending on the source of the parent spec. Instead, this change makes asset file path resolution relative to the "owning spec" i.e. the spec where the asset was created.

This enables workflows such as loading a parent spec via resource provider, then loading a child spec via `from_zip` or in memory providing `spec.assets` and resolution will work as intended.

PiperOrigin-RevId: 903207910
Change-Id: Ia58020ab372a3ceadf31e804d145e2ae53d8e5f9
2026-04-21 07:00:05 -07:00

3350 lines
102 KiB
C++
Raw Blame History

// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Tests for user/user_api.cc.
#include <algorithm>
#include <array>
#include <cctype>
#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/match.h>
#include "src/cc/array_safety.h"
#include <mujoco/mujoco.h>
#include <mujoco/mjspec.h>
#include <mujoco/mjplugin.h>
#include "src/xml/xml_api.h"
#include "src/xml/xml_numeric_format.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::HasSubstr;
using ::testing::NotNull;
using ::testing::IsNull;
// -------------------------- 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);
}
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));
std::strcpy((char*)resource->data, 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);
}
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, 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);
mjs_setName(mat1->element, "yellow");
mjs_setName(mat2->element, "yellow");
EXPECT_EQ(mj_recompile(spec, 0, model, data), -1);
EXPECT_STREQ(mjs_getError(spec), "Error: repeated name 'yellow' 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);
}
// ------------------- test recompilation multiple files -----------------------
std::vector<std::string> GetRecompileTestModels() {
std::vector<std::string> models;
std::string ext(".xml");
for (const auto& path : {GetTestDataFilePath("."), GetModelPath(".")}) {
for (const auto& p : std::filesystem::recursive_directory_iterator(path)) {
if (p.path().extension() == ext) {
std::string xml = p.path().string();
if (absl::StrContains(xml, "malformed_") ||
absl::StrContains(xml, "_fail") ||
absl::StrContains(xml, "touch_grid") ||
absl::StrContains(xml, "perf") ||
absl::StrContains(xml, "cow")) {
continue;
}
models.push_back(xml);
}
}
}
return models;
}
class RecompileCompareTest : public MujocoTest,
public ::testing::WithParamInterface<std::string> {
public:
};
TEST_P(RecompileCompareTest, RecompileCompare) {
std::string xml = GetParam();
std::string field = "";
FullFloatPrecision increase_precision;
// load spec
std::array<char, 1000> err;
mjSpec* s = mj_parseXML(xml.c_str(), 0, err.data(), err.size());
if (!s) {
GTEST_SKIP() << "Failed to load " << xml << ": " << err.data();
}
// copy spec
mjSpec* s_copy = mj_copySpec(s);
// compare signature
EXPECT_EQ(s->element->signature, s_copy->element->signature) << xml;
// compile twice and compare
mjModel* m_old = mj_compile(s, nullptr);
if (!m_old) {
mj_deleteSpec(s);
GTEST_SKIP() << "Failed to compile " << xml << ": " << mjs_getError(s);
}
mjModel* m_new = mj_compile(s, nullptr);
mjModel* m_copy = mj_compile(s_copy, nullptr);
// compare signature
EXPECT_EQ(m_old->signature, m_new->signature) << xml;
EXPECT_EQ(m_old->signature, m_copy->signature) << xml;
ASSERT_THAT(m_new, NotNull())
<< "Failed to recompile " << xml << ": " << mjs_getError(s);
ASSERT_THAT(m_copy, NotNull())
<< "Failed to compile " << xml << ": " << mjs_getError(s_copy);
mjtNum tol = 0;
EXPECT_LE(CompareModel(m_old, m_new, field), tol)
<< "Compiled and recompiled models are different!\n"
<< "Affected file " << xml << '\n'
<< "Different field: " << field << '\n';
EXPECT_LE(CompareModel(m_old, m_copy, field), tol)
<< "Original and copied models are different!\n"
<< "Affected file " << xml << '\n'
<< "Different field: " << field << '\n';
// copy to a new spec, compile and compare
mjSpec* s_copy2 = mj_copySpec(s);
mjModel* m_copy2 = mj_compile(s_copy2, nullptr);
ASSERT_THAT(m_copy2, NotNull())
<< "Failed to compile " << xml << ": " << mjs_getError(s_copy2);
EXPECT_LE(CompareModel(m_old, m_copy2, field), tol)
<< "Original and re-copied models are different!\n"
<< "Affected file " << xml << '\n'
<< "Different field: " << field << '\n';
mj_deleteModel(m_new);
mj_deleteModel(m_copy);
mj_deleteModel(m_copy2);
mj_deleteSpec(s_copy);
mj_deleteSpec(s_copy2);
mj_deleteSpec(s);
mj_deleteModel(m_old);
}
INSTANTIATE_TEST_SUITE_P(
AllModels, RecompileCompareTest,
::testing::ValuesIn(GetRecompileTestModels()),
[](const ::testing::TestParamInfo<std::string>& info) {
std::string name = std::filesystem::path(info.param).filename().string();
std::replace_if(
name.begin(), name.end(),
[](char c) { return !std::isalnum(c); }, '_');
return name + "_" + std::to_string(info.index);
});
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);
}
// ------------------- 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
mjModel* m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_EQ(m->mesh_vert[0], -0.5);
mj_deleteModel(m);
// 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_deleteModel(m);
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
mjModel* 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];
mj_deleteModel(m);
// 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_deleteModel(m);
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
mjModel* 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_deleteModel(m);
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
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';;
// destroy everything
mj_deleteSpec(parent);
mj_deleteModel(m_attached);
mj_deleteModel(m_expected);
}
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());
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
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected, 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);
mj_deleteModel(m_expected);
}
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
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected, 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);
mj_deleteModel(m_expected);
}
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
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(m_detached, m_expected, field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
// destroy everything
mj_deleteSpec(child);
mj_deleteModel(m_detached);
mj_deleteModel(m_expected);
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());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model);
mj_deleteModel(expected);
}
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());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model);
mj_deleteModel(expected);
}
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());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(model2, expected, 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);
mj_deleteModel(expected);
}
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());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, 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);
mj_deleteModel(expected);
}
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());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, 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);
mj_deleteModel(expected);
}
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());
mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull());
// create data
mjData* data = mj_makeData(model);
EXPECT_THAT(data, NotNull());
mjData* d_expected = mj_makeData(m_expected);
EXPECT_THAT(d_expected, NotNull());
// set ctrl
data->ctrl[0] = 1;
data->ctrl[1] = 2;
d_expected->ctrl[0] = 2;
// set mocap
data->mocap_pos[3] = 1;
data->mocap_quat[4] = 0;
data->mocap_quat[5] = 1;
d_expected->mocap_pos[0] = 1;
d_expected->mocap_quat[0] = 0;
d_expected->mocap_quat[1] = 1;
// step models
mj_step(model, data);
mj_step(m_expected, d_expected);
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->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->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->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->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->mocap_pos[3*i+j]) << i;
}
for (int j = 0; j < 4; ++j) {
EXPECT_EQ(data->mocap_quat[4*i+j], d_expected->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_deleteData(d_expected);
mj_deleteSpec(spec);
mj_deleteModel(model);
mj_deleteModel(m_expected);
}
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, 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());
mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(model, m_expected, field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteSpec(spec);
mj_deleteModel(model);
mj_deleteModel(m_expected);
}
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;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, 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, mjOBJ_KEY, 0), "keyframe0");
EXPECT_STREQ(mj_id2name(m, mjOBJ_KEY, 1), "keyframe");
mj_deleteModel(m);
}
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
static char warning[1024];
warning[0] = '\0';
mju_user_warning = [](const char* msg) {
util::strcpy_arr(warning, msg);
};
// attach child to parent
mjs_attach(mjs_findFrame(parent, "frame")->element,
mjs_findBody(child, "body")->element, "child-", "");
EXPECT_THAT(warning, HasSubstr(compile ? "" : "model has pending keyframes"));
// compare models
mjtNum tol = 0;
std::string field = "";
mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull());
mjModel* m_expected = LoadModelFromString(
compile ? expected_xml : expected_xml_uncompiled, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected, 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_expected);
mj_deleteModel(m_attached);
mj_deleteModel(m_child);
}
TEST_F(MujocoTest, TestAttachNestedKeyframe) {
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 = "";
mjModel* expected = LoadModelFromString(xml_expected, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, field), tol)
<< "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteSpec(parent);
mj_deleteSpec(child);
mj_deleteModel(model);
mj_deleteModel(expected);
}
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, 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);
}
} // namespace
} // namespace mujoco
Reference in New Issue View Git Blame Copy Permalink