Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
685c9ca5bThis file is pinned to the selected release. Download the original to open it in your design tool.
// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Tests for user/user_model.cc.
#include <array>
#include <cmath>
#include <cstddef>
#include <memory>
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <absl/strings/str_format.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::ElementsAre;
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::NotNull;
using ::testing::Pointwise;
static std::vector<mjtNum> GetRow(const mjtNum* array, int ncolumn, int row) {
return std::vector<mjtNum>(array + ncolumn * row,
array + ncolumn * (row + 1));
}
// ----------------------------- test mjCModel --------------------------------
using UserModelTest = MujocoTest;
// clarify the semantic of body_rootid and body_weldid
TEST_F(UserModelTest, WeldRootID) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="1">
<joint/>
<geom size="1"/>
<body name="2">
<joint/>
<geom size="1"/>
</body>
</body>
<body name="3">
<joint/>
<geom size="1"/>
<body name="4">
<geom size="1"/>
</body>
</body>
<body name="5">
<geom size="1"/>
<body name="6">
<geom size="1"/>
</body>
</body>
<body name="7" mocap="true">
<geom size="1"/>
<body name="8">
<geom size="1"/>
</body>
</body>
<body name="9" mocap="true">
<geom size="1"/>
<body name="10">
<joint/>
<geom size="1"/>
</body>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, NotNull());
EXPECT_THAT(AsVector(model->body_rootid, model->nbody),
ElementsAre(0, 1, 1, 3, 3, 5, 5, 7, 7, 9, 9));
EXPECT_THAT(AsVector(model->body_weldid, model->nbody),
ElementsAre(0, 1, 2, 3, 3, 0, 0, 0, 0, 0, 10));
mj_deleteModel(model);
}
TEST_F(UserModelTest, RepeatedNames) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="body1">
<joint axis="0 1 0" name="joint1"/>
<geom size="1" name="geom1"/>
<geom size="1" name="geom1"/>
</body>
</worldbody>
</mujoco>)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("repeated name 'geom1' in geom"));
}
TEST_F(UserModelTest, SameFrame) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<geom type="box" size="1 2 3"/>
</default>
<worldbody>
<body name="body1">
<geom name="none" mass="0" pos="1 1 1" euler="10 10 10"/>
<geom name="body" mass="0"/>
<geom name="inertia" mass="1" pos="3 2 1" euler="20 30 40"/>
<geom name="bodyrot" mass="0" pos="1 1 1"/>
<geom name="inertiarot" mass="0" euler="20 30 40"/>
</body>
</worldbody>
</mujoco>)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->geom_sameframe[0], mjSAMEFRAME_NONE);
EXPECT_EQ(model->geom_sameframe[1], mjSAMEFRAME_BODY);
EXPECT_EQ(model->geom_sameframe[2], mjSAMEFRAME_INERTIA);
EXPECT_EQ(model->geom_sameframe[3], mjSAMEFRAME_BODYROT);
EXPECT_EQ(model->geom_sameframe[4], mjSAMEFRAME_INERTIAROT);
// make data, get geom_xpos
mjData* data = mj_makeData(model);
mj_kinematics(model, data);
auto geom_xpos = AsVector(data->geom_xpos, model->ngeom*3);
auto geom_xmat = AsVector(data->geom_xmat, model->ngeom*9);
// set all geom_sameframe to 0, call kinematics again
for (int i = 0; i < model->ngeom; i++) {
model->geom_sameframe[i] = mjSAMEFRAME_NONE;
}
mj_resetData(model, data);
mj_kinematics(model, data);
auto geom_xpos2 = AsVector(data->geom_xpos, model->ngeom*3);
auto geom_xmat2 = AsVector(data->geom_xmat, model->ngeom*9);
// expect them to be equal
constexpr double eps = 1e-6;
EXPECT_THAT(geom_xpos, Pointwise(MjNear(eps, eps), geom_xpos2));
EXPECT_THAT(geom_xmat, Pointwise(MjNear(eps, eps), geom_xmat2));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(UserModelTest, ActuatorSparsity) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="a"/>
<body>
<geom size="1"/>
<joint name="b"/>
</body>
</body>
</worldbody>
<actuator>
<motor joint="a"/>
<motor joint="b"/>
</actuator>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_EQ(m->nJmom, 2);
mj_deleteModel(m);
}
TEST_F(UserModelTest, FixedTendonSparsity) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size=".1"/>
<joint name="0"/>
</body>
<body pos="1 0 0">
<geom size=".1"/>
<joint name="1"/>
</body>
<body pos="2 0 0">
<geom size=".1"/>
<joint name="2"/>
</body>
</worldbody>
<tendon>
<fixed>
<joint coef="3" joint="2"/>
<joint coef="2" joint="1"/>
<joint coef="1" joint="0"/>
</fixed>
</tendon>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
EXPECT_EQ(m->nJten, 3);
EXPECT_EQ(m->ten_J_rownnz[0], 3);
EXPECT_EQ(m->ten_J_rowadr[0], 0);
EXPECT_EQ(m->wrap_type[m->tendon_adr[0]], mjWRAP_JOINT);
int rowadr = m->ten_J_rowadr[0];
int* colind = m->ten_J_colind + rowadr;
EXPECT_THAT(std::vector<int>(colind, colind + 3), ElementsAre(0, 1, 2));
mj_deleteModel(m);
}
TEST_F(UserModelTest, NestedZeroMassBodiesOK) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<freejoint/>
<body>
<body>
<body>
<geom size="1"/>
</body>
</body>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
TEST_F(UserModelTest, NestedZeroMassBodiesWithJointOK) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<freejoint/>
<body>
<body>
<body>
<joint/>
<geom size="1"/>
</body>
<body>
<geom size="1"/>
</body>
</body>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mj_deleteModel(model);
}
TEST_F(UserModelTest, NestedZeroMassBodiesFail) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<body name="bad">
<freejoint/>
<body>
<body>
</body>
</body>
</body>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, IsNull());
EXPECT_THAT(
error,
HasSubstr(
"mass and inertia of moving bodies must be larger than mjMINVAL"));
EXPECT_THAT(error, HasSubstr("Element name 'bad'"));
mj_deleteModel(model);
}
TEST_F(UserModelTest, ConvexHullForCollisionMeshes) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh_no_hull" vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1 0 1 3 0 3 2 1 2 3"/>
<mesh name="mesh_with_hull_contype" vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1 0 1 3 0 3 2 1 2 3"/>
<mesh name="mesh_with_hull_conaffinity" vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1 0 1 3 0 3 2 1 2 3"/>
</asset>
<worldbody>
<geom name="geom_no_hull" type="mesh" mesh="mesh_no_hull" contype="0" conaffinity="0"/>
<geom name="geom_with_hull_contype" type="mesh" mesh="mesh_with_hull_contype" contype="1"/>
<geom name="geom_with_hull_conaffinity" type="mesh" mesh="mesh_with_hull_conaffinity" conaffinity="1"/>
</worldbody>
</mujoco>)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
int no_hull_id = mj_name2id(model, mjOBJ_MESH, "mesh_no_hull");
int with_hull_contype_id =
mj_name2id(model, mjOBJ_MESH, "mesh_with_hull_contype");
int with_hull_conaffinity_id =
mj_name2id(model, mjOBJ_MESH, "mesh_with_hull_conaffinity");
EXPECT_NE(no_hull_id, -1);
EXPECT_NE(with_hull_contype_id, -1);
EXPECT_NE(with_hull_conaffinity_id, -1);
// mesh_no_hull should not have a convex hull.
EXPECT_EQ(model->mesh_graphadr[no_hull_id], -1);
// mesh_with_hull_contype should have a convex hull.
EXPECT_NE(model->mesh_graphadr[with_hull_contype_id], -1);
// mesh_with_hull_conaffinity should have a convex hull.
EXPECT_NE(model->mesh_graphadr[with_hull_conaffinity_id], -1);
mj_deleteModel(model);
}
TEST_F(UserModelTest, ConvexHullForPairCollisionMeshes) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="mesh" vertex="0 0 0 1 0 0 0 1 0 0 0 1"
face="0 2 1 0 1 3 0 3 2 1 2 3"/>
</asset>
<worldbody>
<geom name="geom1" type="sphere" size="1"/>
<geom name="geom_mesh" type="mesh" mesh="mesh" contype="0" conaffinity="0"/>
</worldbody>
<contact>
<pair name="hello" geom1="geom1" geom2="geom_mesh"/>
</contact>
</mujoco>)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
int mesh_in_pair_id = mj_name2id(model, mjOBJ_MESH, "mesh");
EXPECT_NE(mesh_in_pair_id, -1);
// mesh_in_pair should have a convex hull because it is in a collision pair.
EXPECT_NE(model->mesh_graphadr[mesh_in_pair_id], -1);
mj_deleteModel(model);
}
// ------------- test automatic inference of nuser_xxx -------------------------
using UserDataTest = MujocoTest;
TEST_F(UserDataTest, AutoNUserBody) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body user="1 2 3"/>
<body user="2 3"/>
</worldbody>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_body, 3);
EXPECT_THAT(GetRow(m->body_user, m->nuser_body, 1), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->body_user, m->nuser_body, 2), ElementsAre(2, 3, 0));
mj_deleteModel(m);
}
TEST_F(UserDataTest, AutoNUserJoint) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint user="1 2 3"/>
<joint user="2 3"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_jnt, 3);
EXPECT_THAT(GetRow(m->jnt_user, m->nuser_jnt, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->jnt_user, m->nuser_jnt, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
}
TEST_F(UserDataTest, AutoNUserGeom) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom size="1" user="1 2 3"/>
<geom size="1" user="2 3"/>
</worldbody>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_geom, 3);
EXPECT_THAT(GetRow(m->geom_user, m->nuser_geom, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->geom_user, m->nuser_geom, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
}
TEST_F(UserDataTest, AutoNUserSite) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site user="1 2 3"/>
<site user="2 3"/>
</worldbody>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_site, 3);
EXPECT_THAT(GetRow(m->site_user, m->nuser_site, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->site_user, m->nuser_site, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
}
TEST_F(UserDataTest, AutoNUserCamera) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<camera user="1 2 3"/>
<camera user="2 3"/>
</worldbody>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_cam, 3);
EXPECT_THAT(GetRow(m->cam_user, m->nuser_cam, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->cam_user, m->nuser_cam, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
}
TEST_F(UserDataTest, AutoNUserTendon) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site name="a"/>
<site name="b"/>
</worldbody>
<tendon>
<spatial user="1 2 3">
<site site="a"/>
<site site="b"/>
</spatial>
<spatial user="2 3">
<site site="a"/>
<site site="b"/>
</spatial>
</tendon>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_tendon, 3);
EXPECT_THAT(GetRow(m->tendon_user, m->nuser_tendon, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->tendon_user, m->nuser_tendon, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
}
TEST_F(UserDataTest, AutoNUserActuator) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="a"/>
</body>
</worldbody>
<actuator>
<motor joint="a" user="1 2 3"/>
<motor joint="a" user="2 3"/>
</actuator>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_actuator, 3);
EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 0),
ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 1),
ElementsAre(2, 3, 0));
mj_deleteModel(m);
}
TEST_F(UserDataTest, AutoNUserSensor) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site name="a"/>
</worldbody>
<sensor>
<accelerometer site="a" user="1 2 3"/>
<gyro site="a" user="2 3"/>
</sensor>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_sensor, 3);
EXPECT_THAT(GetRow(m->sensor_user, m->nuser_sensor, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->sensor_user, m->nuser_sensor, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
}
// ------------- test duplicate names ------------------------------------------
TEST_F(UserDataTest, DuplicateNames) {
static const char* const kFilePath = "user/testdata/malformed_duplicated.xml";
const std::string xml_path = GetTestDataFilePath(kFilePath);
std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
EXPECT_THAT(m, IsNull());
EXPECT_STREQ(error.data(), "Error: repeated name 'cube' in mesh");
}
// ------------- test fusestatic -----------------------------------------------
using FuseStaticTest = MujocoTest;
TEST_F(FuseStaticTest, FuseStaticEquivalent) {
static constexpr char xml_template[] = R"(
<mujoco>
<compiler fusestatic="%s"/>
<worldbody>
<body>
<joint axis="1 0 0"/>
<geom size="0.5" pos="1 0 0" contype="0" conaffinity="0"/>
<body>
<geom size="0.5" pos="0 1 0" contype="1" conaffinity="1"/>
<geom size="0.5" pos="0 -2 0" contype="1" conaffinity="1"/>
</body>
</body>
</worldbody>
</mujoco>
)";
std::string fuse = absl::StrFormat(xml_template, "true");
std::string no_fuse = absl::StrFormat(xml_template, "false");
mjModel* m_fuse = LoadModelFromString(fuse.c_str());
mjModel* m_no_fuse = LoadModelFromString(no_fuse.c_str());
ASSERT_THAT(m_fuse, NotNull());
ASSERT_THAT(m_no_fuse, NotNull());
EXPECT_EQ(m_fuse->nbody, 2) << "Expecting a world body and one other body";
EXPECT_EQ(m_no_fuse->nbody, 3) << "Expecting a world body and two others";
EXPECT_EQ(m_no_fuse->body_contype[2], 1);
EXPECT_EQ(m_no_fuse->body_conaffinity[2], 1);
EXPECT_EQ(m_fuse->body_contype[1], 1);
EXPECT_EQ(m_fuse->body_conaffinity[1], 1);
EXPECT_EQ(m_no_fuse->body_bvhnum[2], 3);
EXPECT_EQ(m_fuse->body_bvhnum[1], 3);
mjData* d_fuse = mj_makeData(m_fuse);
mjData* d_no_fuse = mj_makeData(m_no_fuse);
mj_step(m_fuse, d_fuse);
mj_step(m_no_fuse, d_no_fuse);
EXPECT_NEAR(d_fuse->qvel[0], d_no_fuse->qvel[0], MjTol(2e-17, 1e-8))
<< "Velocity should be the same after 1 step";
EXPECT_NE(d_fuse->qvel[0], 0);
mj_deleteData(d_fuse);
mj_deleteData(d_no_fuse);
mj_deleteModel(m_fuse);
mj_deleteModel(m_no_fuse);
}
TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) {
static constexpr char xml_template[] = R"(
<mujoco>
<compiler fusestatic="true"/>
<worldbody>
<body>
<joint axis="1 0 0"/>
<geom size="0.5" pos="1 0 0" contype="0" conaffinity="0"/>
<body name="not_referenced">
<geom size="0.5" pos="0 1 0" contype="1" conaffinity="1"/>
<geom size="0.5" pos="0 -2 0" contype="1" conaffinity="1"/>
</body>
<body name="referenced">
<geom size="0.5" pos="0 1 0" contype="1" conaffinity="1"/>
<geom size="0.5" pos="0 -2 0" contype="1" conaffinity="1"/>
</body>
</body>
</worldbody>
<actuator>
<adhesion body="referenced" ctrlrange="0 1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml_template, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
mj_deleteModel(m);
}
TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) {
static constexpr char xml_template[] = R"(
<mujoco>
<compiler fusestatic="true"/>
<worldbody>
<light mode="targetbody" target="referenced"/>
<body>
<joint axis="1 0 0"/>
<geom size="0.5" pos="1 0 0" contype="0" conaffinity="0"/>
<body name="not_referenced">
<geom size="0.5" pos="0 1 0" contype="1" conaffinity="1"/>
<geom size="0.5" pos="0 -2 0" contype="1" conaffinity="1"/>
</body>
<body name="referenced">
<geom size="0.5" pos="0 1 0" contype="1" conaffinity="1"/>
<geom size="0.5" pos="0 -2 0" contype="1" conaffinity="1"/>
</body>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml_template, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
mj_deleteModel(m);
}
TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) {
static constexpr char xml_template[] = R"(
<mujoco>
<compiler fusestatic="true"/>
<worldbody>
<body>
<joint axis="1 0 0"/>
<geom size="0.5" pos="1 0 0" contype="0" conaffinity="0"/>
<body name="not_referenced">
<geom size="0.5" pos="0 1 0" contype="1" conaffinity="1"/>
<geom size="0.5" pos="0 -2 0" contype="1" conaffinity="1"/>
</body>
<body name="referenced">
<site name="force"/>
<geom size="0.5" pos="0 1 0" contype="1" conaffinity="1"/>
<geom size="0.5" pos="0 -2 0" contype="1" conaffinity="1"/>
</body>
</body>
</worldbody>
<sensor>
<force site="force"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml_template, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
mj_deleteModel(m);
}
TEST_F(FuseStaticTest, FuseStaticCameraInBody) {
static constexpr char xml[] = R"(
<mujoco>
<compiler fusestatic="true"/>
<worldbody>
<body>
<joint axis="1 0 0"/>
<geom size="0.5"/>
<body pos="1 0 0">
<site name="site1"/>
<camera name="cam1"/>
</body>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
EXPECT_EQ(m->nbody, 2) << "Static body should be fused";
EXPECT_EQ(m->ncam, 1);
mj_deleteModel(m);
}
TEST_F(FuseStaticTest, FuseStaticLightInBody) {
static constexpr char xml[] = R"(
<mujoco>
<compiler fusestatic="true"/>
<worldbody>
<body>
<joint axis="1 0 0"/>
<geom size="0.5"/>
<body pos="1 0 0">
<light name="light1" dir="0 0 -1"/>
</body>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
EXPECT_EQ(m->nbody, 2) << "Static body should be fused";
EXPECT_EQ(m->nlight, 1);
mj_deleteModel(m);
}
// ------------- test discardvisual --------------------------------------------
using DiscardVisualTest = MujocoTest;
TEST_F(DiscardVisualTest, DiscardVisualKeepsInertia) {
static constexpr char xml[] = R"(
<mujoco>
<compiler discardvisual="true"/>
<asset>
<mesh name="visual_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
normal="1 0 0 0 1 0 0 0 1 0.707 0 0.707"
face="0 2 1 0 3 2" />
<mesh name="collision_mesh"
vertex="0 0 0 1 0 0 0 1 0 0 0 1"
normal="1 0 0 0 1 0 0 0 1 0.707 0 0.707"
face="0 2 1 0 3 2" />
</asset>
<worldbody>
<body>
<geom type="mesh" mesh="visual_mesh" contype="0" conaffinity="0"/>
</body>
<body>
<geom type="mesh" mesh="collision_mesh"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, NotNull()) << error.data();
EXPECT_THAT(model->nmesh, 1);
EXPECT_THAT(model->body_inertia[3], model->body_inertia[6]);
EXPECT_THAT(model->body_inertia[4], model->body_inertia[7]);
EXPECT_THAT(model->body_inertia[5], model->body_inertia[8]);
mj_deleteModel(model);
}
TEST_F(DiscardVisualTest, DiscardVisualEquivalent) {
char error[1024];
size_t error_sz = 1024;
static const char* const kDiscardvisualPath =
"user/testdata/discardvisual.xml";
static const char* const kDiscardvisualFalsePath =
"user/testdata/discardvisual_false.xml";
const std::string xml_path1 = GetTestDataFilePath(kDiscardvisualPath);
mjModel* model1 = mj_loadXML(xml_path1.c_str(), 0, error, error_sz);
EXPECT_THAT(model1, NotNull()) << error;
const std::string xml_path2 = GetTestDataFilePath(kDiscardvisualFalsePath);
mjModel* model2 = mj_loadXML(xml_path2.c_str(), 0, error, error_sz);
EXPECT_THAT(model2, NotNull()) << error;
EXPECT_THAT(model1->nq, model2->nq);
EXPECT_THAT(model1->nmat, 0);
EXPECT_THAT(model1->ntex, 0);
EXPECT_THAT(model2->ngeom-model1->ngeom, 3);
EXPECT_THAT(model2->nmesh-model1->nmesh, 2);
EXPECT_THAT(model1->npair, model2->npair);
EXPECT_THAT(model1->nsensor, model2->nsensor);
EXPECT_THAT(model1->nwrap, model2->nwrap);
for (int i = 0; i < model1->ngeom; i++) {
std::string name = std::string(model1->names + model1->name_geomadr[i]);
EXPECT_NE(name.find("kept"), std::string::npos);
EXPECT_EQ(name.find("discard"), std::string::npos);
}
for (int i = 0; i < model1->npair; i++) {
int adr1 = model1->name_geomadr[model1->pair_geom1[i]];
int adr2 = model2->name_geomadr[model2->pair_geom1[i]];
EXPECT_STREQ(model1->names + adr1, model2->names + adr2);
adr1 = model1->name_geomadr[model1->pair_geom2[i]];
adr2 = model2->name_geomadr[model2->pair_geom2[i]];
EXPECT_STREQ(model1->names + adr1, model2->names + adr2);
}
for (int i = 0; i < model1->nsensor; i++) {
int adr1 = model1->name_geomadr[model1->sensor_objid[i]];
int adr2 = model2->name_geomadr[model2->sensor_objid[i]];
EXPECT_STREQ(model1->names + adr1, model2->names + adr2);
}
for (int i = 0; i < model1->nwrap; i++) {
int adr1 = model1->name_geomadr[model1->wrap_objid[i]];
int adr2 = model2->name_geomadr[model2->wrap_objid[i]];
EXPECT_STREQ(model1->names + adr1, model2->names + adr2);
}
mjData *d1 = mj_makeData(model1);
mjData *d2 = mj_makeData(model2);
for (int i = 0; i < 100; i++) {
mj_step(model1, d1);
mj_step(model2, d2);
}
for (int i = 0; i < model1->nq; i++) {
EXPECT_THAT(d1->qpos[i], d2->qpos[i]);
}
mj_deleteModel(model1);
mj_deleteModel(model2);
mj_deleteData(d1);
mj_deleteData(d2);
}
// ------------- test lengthrange ----------------------------------------------
using LengthRangeTest = MujocoTest;
TEST_F(LengthRangeTest, LengthRangeThreading) {
char error[1024];
size_t error_sz = 1024;
std::string field = "";
static const char* const kLengthrangePath =
"user/testdata/lengthrange.xml";
const std::string xml_path1 = GetTestDataFilePath(kLengthrangePath);
mjSpec* spec = mj_parseXML(xml_path1.c_str(), 0, error, error_sz);
EXPECT_THAT(spec, NotNull()) << error;
mjModel* model1 = mj_compile(spec, 0);
EXPECT_THAT(model1, NotNull()) << error;
// model is such that the lengthrange for first actuator is [1, sqrt(5)]
EXPECT_NEAR(model1->actuator_lengthrange[0], 1.0, 1e-3);
EXPECT_NEAR(model1->actuator_lengthrange[1], std::sqrt(5.0), 1e-3);
// recompile without threads
ASSERT_EQ(spec->compiler.usethread, 1);
spec->compiler.usethread = 0;
mjModel* model2 = mj_compile(spec, 0);
EXPECT_THAT(model2, NotNull()) << error;
// expect threaded and unthreaded models to be identical
EXPECT_LE(CompareModel(model1, model2, field), 0)
<< "Threaded and unthreaded lengthrange models are different!\n"
<< "Different field: " << field << '\n';
mj_deleteModel(model1);
mj_deleteModel(model2);
mj_deleteSpec(spec);
}
// ----------------------------- test modeldir --------------------------------
TEST_F(MujocoTest, Modeldir) {
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(), "meshdir/cube.obj", cube, sizeof(cube));
// child with the asset
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_setString(child->compiler.meshdir, "meshdir");
mjs_setString(mesh->file, "cube.obj");
mjs_setName(mesh->element, "cube");
mjs_setString(geom->meshname, "cube");
mjs_setFrame(geom->element, frame);
geom->type = mjGEOM_MESH;
// parent attaching the child
mjSpec* spec = mj_makeSpec();
mjs_setDeepCopy(spec, true);
mjs_setString(spec->compiler.meshdir, "asset");
mjs_attach(mjs_findBody(spec, "world")->element, frame->element, "_", "");
mjModel* model = mj_compile(spec, vfs.get());
EXPECT_THAT(model, NotNull());
mj_deleteSpec(child);
mj_deleteSpec(spec);
mj_deleteModel(model);
mj_deleteVFS(vfs.get());
}
TEST_F(MujocoTest, NestedMeshDir) {
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)";
static constexpr char child_xml[] = R"(
<mujoco>
<compiler meshdir="child_meshdir"/>
<asset>
<mesh name="m" file="child_mesh.obj"/>
</asset>
<worldbody>
<body name="child">
<geom type="mesh" mesh="m"/>
</body>
</worldbody>
</mujoco>
)";
static constexpr char parent_xml[] = R"(
<mujoco>
<compiler meshdir="parent_meshdir"/>
<asset>
<mesh name="m" file="parent_mesh.obj"/>
<model name="child" file="child.xml"/>
</asset>
<worldbody>
<body name="parent">
<geom type="mesh" mesh="m"/>
<attach model="child" body="child" prefix="child_"/>
</body>
</worldbody>
</mujoco>
)";
static constexpr char grandparent_xml[] = R"(
<mujoco>
<compiler meshdir="grandparent_meshdir"/>
<asset>
<mesh name="m" file="grandparent_mesh.obj"/>
<model name="parent" file="parent.xml"/>
</asset>
<worldbody>
<geom type="mesh" mesh="m"/>
<attach model="parent" body="parent" prefix="parent_"/>
</worldbody>
</mujoco>
)";
auto vfs = std::make_unique<mjVFS>();
mj_defaultVFS(vfs.get());
mj_addBufferVFS(vfs.get(), "child_meshdir/child_mesh.obj", cube,
sizeof(cube));
mj_addBufferVFS(vfs.get(), "child.xml", child_xml, sizeof(child_xml));
mj_addBufferVFS(vfs.get(), "parent_meshdir/parent_mesh.obj", cube,
sizeof(cube));
mj_addBufferVFS(vfs.get(), "parent.xml", parent_xml, sizeof(parent_xml));
mj_addBufferVFS(vfs.get(), "grandparent_meshdir/grandparent_mesh.obj", cube,
sizeof(cube));
std::array<char, 1024> error;
mjModel* child_model = LoadModelFromString(child_xml, error.data(),
error.size(), vfs.get());
EXPECT_THAT(child_model, NotNull()) << error.data();
mj_deleteModel(child_model);
mjModel* parent_model = LoadModelFromString(parent_xml, error.data(),
error.size(), vfs.get());
EXPECT_THAT(parent_model, NotNull()) << error.data();
mj_deleteModel(parent_model);
mjModel* grandparent_model = LoadModelFromString(
grandparent_xml, error.data(), error.size(), vfs.get());
EXPECT_THAT(grandparent_model, NotNull()) << error.data();
mj_deleteModel(grandparent_model);
mj_deleteVFS(vfs.get());
}
TEST_F(MujocoTest, ConvertSpringdamper) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint axis="0 1 0" springdamper="1 1"/>
<geom size="0.2 0.2 0.2" type="box"/>
</body>
</worldbody>
</mujoco>
)";
std::array<char, 1024> err;
mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size());
ASSERT_THAT(spec, NotNull()) << err.data();
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << err.data();
std::array<char, 1024> str;
mj_saveXMLString(spec, str.data(), str.size(), err.data(), err.size());
EXPECT_THAT(str.data(), HasSubstr("damping"));
EXPECT_THAT(str.data(), HasSubstr("stiffness"));
mj_deleteModel(model);
mj_deleteSpec(spec);
}
// ------------- test history buffer computation -------------------------------
using DelayBufferTest = MujocoTest;
TEST_F(DelayBufferTest, ActuatorDelayBufferSizes) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="1"/>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt1"/>
<joint name="jnt2"/>
<joint name="jnt3"/>
</body>
</worldbody>
<actuator>
<motor joint="jnt1"/>
<motor joint="jnt2" delay="3" nsample="3"/>
<motor joint="jnt3" delay="10" nsample="10"/>
</actuator>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
ASSERT_EQ(m->nu, 3);
// nhistory = (2+2*3) + (2+2*10) = 8 + 22 = 30
EXPECT_EQ(m->nhistory, 30);
// verify per-actuator delay and addresses
EXPECT_EQ(m->actuator_history[0], 0);
EXPECT_EQ(m->actuator_history[2], 3);
EXPECT_EQ(m->actuator_history[4], 10);
EXPECT_EQ(m->actuator_historyadr[0], -1);
EXPECT_EQ(m->actuator_historyadr[1], 0);
EXPECT_EQ(m->actuator_historyadr[2], 8);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco