Alessio Quaglino
c331d18a6e
PiperOrigin-RevId: 626001651 Change-Id: I3dc52baa6aad875504dd4c7d969a4091f52c4c16
517 lines
16 KiB
C++
517 lines
16 KiB
C++
// Copyright 2021 DeepMind Technologies Limited
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
// Tests for user/user_api.cc.
|
|
|
|
#include <array>
|
|
#include <filesystem>
|
|
#include <memory>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include <gmock/gmock.h>
|
|
#include <gtest/gtest.h>
|
|
#include <absl/strings/match.h>
|
|
#include <mujoco/mujoco.h>
|
|
#include "src/user/user_api.h"
|
|
#include "src/xml/xml.h"
|
|
#include "src/xml/xml_numeric_format.h"
|
|
#include "test/fixture.h"
|
|
|
|
namespace mujoco {
|
|
namespace {
|
|
|
|
using ::testing::HasSubstr;
|
|
using ::testing::NotNull;
|
|
|
|
|
|
// ----------------------------- test set/get --------------------------------
|
|
|
|
TEST_F(MujocoTest, ReadWriteData) {
|
|
mjSpec* spec = mjs_createSpec();
|
|
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_setString(site->name, str);
|
|
mjs_setDouble(site->userdata, vec, 10);
|
|
}
|
|
|
|
EXPECT_THAT(mjs_getString(site->name), HasSubstr("sitename"));
|
|
|
|
int nsize;
|
|
const double* vec = mjs_getDouble(site->userdata, &nsize);
|
|
for (int i = 0; i < nsize; ++i) {
|
|
EXPECT_EQ(vec[i], i);
|
|
}
|
|
|
|
mjs_deleteSpec(spec);
|
|
}
|
|
|
|
// ------------------- test recompilation multiple files ----------------------
|
|
TEST_F(PluginTest, RecompileCompare) {
|
|
mjtNum tol = 0;
|
|
std::string field = "";
|
|
|
|
// full precision float printing
|
|
FullFloatPrecision increase_precision;
|
|
|
|
// loop over all xml files in data
|
|
std::vector<std::string> paths = {GetTestDataFilePath("."),
|
|
GetModelPath(".")};
|
|
std::string ext(".xml");
|
|
for (auto const& path : paths) {
|
|
for (auto &p : std::filesystem::recursive_directory_iterator(path)) {
|
|
if (p.path().extension() == ext) {
|
|
std::string xml = p.path().string();
|
|
|
|
// if file is meant to fail, skip it
|
|
if (absl::StrContains(p.path().string(), "malformed_") ||
|
|
absl::StrContains(p.path().string(), "touch_grid") ||
|
|
absl::StrContains(p.path().string(), "cow") ||
|
|
absl::StrContains(p.path().string(), "discardvisual")) {
|
|
continue;
|
|
}
|
|
|
|
// load spec
|
|
std::array<char, 1000> err;
|
|
mjSpec* s = mjParseXML(xml.c_str(), nullptr, err.data(), err.size());
|
|
|
|
// copy spec
|
|
mjSpec* s_copy = mjs_copySpec(s);
|
|
|
|
// compile twice and compare
|
|
mjModel* m_old = mjs_compile(s, nullptr);
|
|
mjModel* m_new = mjs_compile(s, nullptr);
|
|
mjModel* m_copy = mjs_compile(s_copy, nullptr);
|
|
|
|
ASSERT_THAT(m_old, NotNull())
|
|
<< "Failed to compile " << xml << ": " << err.data();
|
|
ASSERT_THAT(m_new, NotNull())
|
|
<< "Failed to recompile " << xml << ": " << err.data();
|
|
ASSERT_THAT(m_copy, NotNull())
|
|
<< "Failed to compile " << xml << ": " << err.data();
|
|
|
|
EXPECT_LE(CompareModel(m_old, m_new, field), tol)
|
|
<< "Compiled and recompiled models are different!\n"
|
|
<< "Affected file " << p.path().string() << '\n'
|
|
<< "Different field: " << field << '\n';
|
|
|
|
EXPECT_LE(CompareModel(m_old, m_copy, field), tol)
|
|
<< "Original and copied models are different!\n"
|
|
<< "Affected file " << p.path().string() << '\n'
|
|
<< "Different field: " << field << '\n';
|
|
|
|
// copy to a new spec, compile and compare
|
|
mjSpec* s_copy2 = mjs_copySpec(s);
|
|
mjModel* m_copy2 = mjs_compile(s_copy2, nullptr);
|
|
|
|
ASSERT_THAT(m_copy2, NotNull())
|
|
<< "Failed to compile " << xml << ": " << err.data();
|
|
|
|
EXPECT_LE(CompareModel(m_old, m_copy2, field), tol)
|
|
<< "Original and re-copied models are different!\n"
|
|
<< "Affected file " << p.path().string() << '\n'
|
|
<< "Different field: " << field << '\n';
|
|
|
|
// delete models
|
|
mjs_deleteSpec(s);
|
|
mjs_deleteSpec(s_copy);
|
|
mjs_deleteSpec(s_copy2);
|
|
mj_deleteModel(m_old);
|
|
mj_deleteModel(m_new);
|
|
mj_deleteModel(m_copy);
|
|
mj_deleteModel(m_copy2);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// ------------------- test cache with modified assets -------------------------
|
|
TEST_F(PluginTest, RecompileCompareCache) {
|
|
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());
|
|
}
|
|
|
|
// -------------------------------- test attach -------------------------------
|
|
static constexpr char xml_child[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<frame name="pframe">
|
|
<frame name="cframe">
|
|
<body name="body">
|
|
<joint type="hinge" name="hinge"/>
|
|
<geom type="cylinder" size=".1 1 0"/>
|
|
<light mode="targetbody" target="targetbody"/>
|
|
<body name="targetbody"/>
|
|
<body/>
|
|
</body>
|
|
</frame>
|
|
</frame>
|
|
<body name="ignore"/>
|
|
<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"/>
|
|
<position name="fixed" tendon="fixed"/>
|
|
</actuator>
|
|
<contact>
|
|
<exclude body1="body" body2="targetbody"/>
|
|
</contact>
|
|
</mujoco>)";
|
|
|
|
TEST_F(MujocoTest, AttachSame) {
|
|
std::array<char, 1000> er;
|
|
mjtNum tol = 0;
|
|
std::string field = "";
|
|
|
|
static constexpr char xml_result[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="body">
|
|
<joint type="hinge" name="hinge"/>
|
|
<geom type="cylinder" size=".1 1 0"/>
|
|
<light mode="targetbody" target="targetbody"/>
|
|
<body name="targetbody"/>
|
|
<body/>
|
|
</body>
|
|
<body name="ignore"/>
|
|
<frame name="frame" pos=".1 0 0" euler="0 90 0">
|
|
<body name="attached-body-1">
|
|
<joint type="hinge" name="attached-hinge-1"/>
|
|
<geom type="cylinder" size=".1 1 0"/>
|
|
<light mode="targetbody" target="attached-targetbody-1"/>
|
|
<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"/>
|
|
<position name="fixed" tendon="fixed"/>
|
|
<position name="attached-hinge-1" joint="attached-hinge-1"/>
|
|
<position name="attached-fixed-1" tendon="attached-fixed-1"/>
|
|
</actuator>
|
|
<contact>
|
|
<exclude body1="body" body2="targetbody"/>
|
|
<exclude body1="attached-body-1" body2="attached-targetbody-1"/>
|
|
</contact>
|
|
</mujoco>)";
|
|
|
|
// create parent
|
|
mjSpec* parent = ParseSpecFromString(xml_child, er.data(), er.size());
|
|
EXPECT_THAT(parent, NotNull()) << er.data();
|
|
|
|
// 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
|
|
EXPECT_THAT(
|
|
mjs_attachBody(frame, body, /*prefix=*/"attached-", /*suffix=*/"-1"), 0);
|
|
|
|
// compile new model
|
|
mjModel* m_attached = mjs_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
|
|
mjs_deleteSpec(parent);
|
|
mj_deleteModel(m_attached);
|
|
mj_deleteModel(m_expected);
|
|
}
|
|
|
|
TEST_F(MujocoTest, AttachDifferent) {
|
|
std::array<char, 1000> er;
|
|
mjtNum tol = 0;
|
|
std::string field = "";
|
|
|
|
static constexpr char xml_parent[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="sphere">
|
|
<freejoint/>
|
|
<geom size=".1"/>
|
|
<frame name="frame" pos=".1 0 0" euler="0 90 0"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>)";
|
|
|
|
static constexpr char xml_result[] = R"(
|
|
<mujoco>
|
|
<worldbody>
|
|
<body name="sphere">
|
|
<freejoint/>
|
|
<geom size=".1"/>
|
|
<frame name="frame" pos=".1 0 0" euler="0 90 0">
|
|
<body name="attached-body-1">
|
|
<joint type="hinge" name="attached-hinge-1"/>
|
|
<geom type="cylinder" size=".1 1 0"/>
|
|
<light mode="targetbody" target="attached-targetbody-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"/>
|
|
<position name="attached-fixed-1" tendon="attached-fixed-1"/>
|
|
</actuator>
|
|
<contact>
|
|
<exclude body1="attached-body-1" body2="attached-targetbody-1"/>
|
|
</contact>
|
|
</mujoco>)";
|
|
|
|
// model with one free sphere and a frame
|
|
mjSpec* parent = ParseSpecFromString(xml_parent, 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 = ParseSpecFromString(xml_child, 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
|
|
EXPECT_EQ(
|
|
mjs_attachBody(frame, body, /*prefix=*/"attached-", /*suffix=*/"-1"), 0);
|
|
|
|
// compile new model
|
|
mjModel* m_attached = mjs_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
|
|
mjs_deleteSpec(parent);
|
|
mjs_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>
|
|
<worldbody>
|
|
<body name="sphere">
|
|
<freejoint/>
|
|
<geom size=".1"/>
|
|
<frame name="frame" pos=".1 0 0" euler="0 90 0"/>
|
|
</body>
|
|
</worldbody>
|
|
</mujoco>)";
|
|
|
|
static constexpr char xml_result[] = R"(
|
|
<mujoco>
|
|
<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 type="cylinder" size=".1 1 0"/>
|
|
<light mode="targetbody" target="attached-targetbody-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"/>
|
|
<position name="attached-fixed-1" tendon="attached-fixed-1"/>
|
|
</actuator>
|
|
<contact>
|
|
<exclude body1="attached-body-1" body2="attached-targetbody-1"/>
|
|
</contact>
|
|
</mujoco>)";
|
|
|
|
// model with one free sphere and a frame
|
|
mjSpec* parent = ParseSpecFromString(xml_parent, er.data(), er.size());
|
|
EXPECT_THAT(parent, NotNull()) << er.data();
|
|
|
|
// get frame
|
|
mjsBody* body = mjs_findBody(parent, "sphere");
|
|
EXPECT_THAT(body, NotNull());
|
|
|
|
// model with one cylinder and a hinge
|
|
mjSpec* child = ParseSpecFromString(xml_child, er.data(), er.size());
|
|
EXPECT_THAT(child, NotNull()) << er.data();
|
|
|
|
// get subtree
|
|
mjsFrame* frame = mjs_findFrame(child, "pframe");
|
|
EXPECT_THAT(frame, NotNull());
|
|
|
|
// attach child to parent frame
|
|
EXPECT_THAT(
|
|
mjs_attachFrame(body, frame, /*prefix=*/"attached-", /*suffix=*/"-1"), 0);
|
|
|
|
// compile new model
|
|
mjModel* m_attached = mjs_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
|
|
mjs_deleteSpec(parent);
|
|
mjs_deleteSpec(child);
|
|
mj_deleteModel(m_attached);
|
|
mj_deleteModel(m_expected);
|
|
}
|
|
|
|
} // namespace
|
|
} // namespace mujoco
|