chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add replicate element to MJCF. Fixes #553

Browse Source 
PiperOrigin-RevId: 629393578
Change-Id: Ia1db1b9e47eba8868117b0f7b85edcd0cc1b6d79
This commit is contained in:
Alessio Quaglino
2024-04-30 06:28:09 -07:00
committed by Copybara-Service
parent 6a5bbb5157
commit 26f23066d1
30 changed files with 8756 additions and 2807 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_plugin_test.cc
+1 -1
View File
@@ -560,7 +560,7 @@ TEST_F(EnginePluginTest, SensorPlugin) {
testing::ElementsAreArray<int>({3*(i+1), 6*j, 3*j}));
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[3]>(d->plugin_state +
m->plugin_stateadr[4]),
testing::ElementsAreArray<int>({4*(i+1), 8*j, 4*j}));
testing::ElementsAreArray<int>({5*(i+1), 10*j, 5*j}));
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[18]>(d->sensordata),
testing::ElementsAreArray<int>({ i+1, 2*j, j,
5*(i+1), 10*j, 5*j,
test/fixture.cc
+14 -8
View File
@@ -191,7 +191,7 @@ auto Compare(T val1, T val2) {
ReturnType magnitude = mju_abs(val1) + mju_abs(val2);
error = mju_abs(val1/magnitude - val2/magnitude) / magnitude;
}
ReturnType safety_factor = 10;
ReturnType safety_factor = 200;
return error < safety_factor * std::numeric_limits<ReturnType>::epsilon()
? 0
: error;
@@ -213,13 +213,19 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2,
if (maxdif > 0) return maxdif;
// compare arrays
#define X(type, name, nr, nc) \
for (int r=0; r < m1->nr; r++) \
for (int c=0; c < nc; c++) { \
dif = Compare(m1->name[r*nc+c], m2->name[r*nc+c]); \
if (dif > maxdif) { maxdif = dif; field = #name;} }
MJMODEL_POINTERS
#undef X
#define X(type, name, nr, nc) \
for (int r = 0; r < m1->nr; r++) \
for (int c = 0; c < nc; c++) { \
dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \
if (dif > maxdif) { \
maxdif = dif; \
field = #name; \
field += " row: " + std::to_string(r); \
field += " col: " + std::to_string(c); \
} \
} // NOLINT
MJMODEL_POINTERS
#undef X
// compare scalars in mjOption
#define X(type, name) \
test/user/user_api_test.cc
+30 -6
View File
@@ -204,12 +204,17 @@ static constexpr char xml_child[] = R"(
</default>
</default>
<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"/>
<geom class="cylinder" material="material"/>
<light mode="targetbody" target="targetbody"/>
<body name="targetbody"/>
<body/>
@@ -254,10 +259,15 @@ TEST_F(MujocoTest, AttachSame) {
</default>
</default>
<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"/>
<geom class="cylinder" material="material"/>
<light mode="targetbody" target="targetbody"/>
<body name="targetbody"/>
<body/>
@@ -266,7 +276,7 @@ TEST_F(MujocoTest, AttachSame) {
<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"/>
<geom class="cylinder" material="material"/>
<light mode="targetbody" target="attached-targetbody-1"/>
<body name="attached-targetbody-1"/>
<body/>
@@ -374,6 +384,11 @@ TEST_F(MujocoTest, AttachDifferent) {
</default>
</default>
<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/>
@@ -381,7 +396,7 @@ TEST_F(MujocoTest, AttachDifferent) {
<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"/>
<geom class="attached-cylinder-1" material="attached-material-1"/>
<light mode="targetbody" target="attached-targetbody-1"/>
<body name="attached-targetbody-1"/>
<body/>
@@ -478,6 +493,11 @@ TEST_F(MujocoTest, AttachFrame) {
</default>
</default>
<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/>
@@ -487,7 +507,7 @@ TEST_F(MujocoTest, AttachFrame) {
<frame name="cframe">
<body name="attached-body-1">
<joint type="hinge" name="attached-hinge-1"/>
<geom class="attached-cylinder-1"/>
<geom class="attached-cylinder-1" material="attached-material-1"/>
<light mode="targetbody" target="attached-targetbody-1"/>
<body name="attached-targetbody-1"/>
<body/>
@@ -568,6 +588,11 @@ TEST_F(MujocoTest, DetachBody) {
static constexpr char xml_result[] = R"(
<mujoco>
<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">
@@ -605,7 +630,6 @@ TEST_F(MujocoTest, DetachBody) {
// destroy everything
mjs_deleteSpec(child);
mjs_deleteBody(body);
mj_deleteModel(m_detached);
mj_deleteModel(m_expected);
}
test/xml/xml_native_reader_test.cc
+167
View File
@@ -27,6 +27,8 @@
#include <mujoco/mujoco.h>
#include "src/cc/array_safety.h"
#include "src/engine/engine_util_errmem.h"
#include "src/user/user_api.h"
#include "src/xml/xml.h"
#include "test/fixture.h"
namespace mujoco {
@@ -888,6 +890,171 @@ TEST_F(XMLReaderTest, DuplicateFrameName) {
EXPECT_THAT(error.data(), HasSubstr("repeated name 'frame1'"));
}
// ---------------------- test replicate parsing -------------------------------
TEST_F(XMLReaderTest, ParseReplicate) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<texture name="texture" type="2d" builtin="checker" width="32" height="32"/>
<material name="material" texture="texture" texrepeat="1 1" texuniform="true"/>
</asset>
<worldbody>
<replicate count="2" offset="1 0 0">
<replicate count="2" offset="0 1 0" sep="_">
<geom name="geom" size="1" pos="0 0 1" material="material"/>
<site name="site" pos="1 0 0"/>
</replicate>
</replicate>
<replicate count="101" euler="0 0 1.8">
<geom name="g" size="1" pos="0 -1 0"/>
</replicate>
</worldbody>
<sensor>
<framepos name="sensor" objtype="site" objname="site"/>
</sensor>
</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, 105);
EXPECT_THAT(m->nsensor, 4);
// check that the separator is used correctly
for (int i = 0; i < 2; ++i) {
for (int j = 0; j < 2; ++j) {
char geom_name[mjMAXUINAME] = "";
util::strcat_arr(geom_name, m->names + m->name_geomadr[2*i+j]);
EXPECT_THAT(m->geom_pos[6*i+3*j+0], i);
EXPECT_THAT(m->geom_pos[6*i+3*j+1], j);
EXPECT_THAT(m->geom_pos[6*i+3*j+2], 1);
EXPECT_THAT(std::string(geom_name),
"geom_" + std::to_string(j) + std::to_string(i));
}
}
// check that 3 digits are used if count > 99
for (int i = 0; i < 2; ++i) {
for (int j = 0; j < 10; ++j) {
for (int k = 0; k < 10; ++k) {
int ngeom = 100*i+10*j+k;
if (ngeom > 99) break;
char geom_name[mjMAXUINAME] = "";
util::strcat_arr(geom_name, m->names + m->name_geomadr[4+ngeom]);
EXPECT_THAT(
std::string(geom_name),
"g" + std::to_string(i) + std::to_string(j) + std::to_string(k));
}
}
}
// check that the final pose is correct
int n = 104;
EXPECT_NEAR(m->geom_pos[3*n+0], 0, 1e-8);
EXPECT_NEAR(m->geom_pos[3*n+1], 1, 1e-8);
EXPECT_EQ(m->geom_pos[3*n+2], 0);
EXPECT_NEAR(m->geom_quat[4*n+0], 0, 1e-8);
EXPECT_EQ(m->geom_quat[4*n+1], 0);
EXPECT_EQ(m->geom_quat[4*n+2], 0);
EXPECT_EQ(m->geom_quat[4*n+3], 1);
mj_deleteModel(m);
}
TEST_F(XMLReaderTest, ParseReplicatePartialReference) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom name="floor" type="plane" size="1 1 1"/>
<site name="world"/>
<replicate count="2" offset="1 0 0">
<body name="replicated">
<freejoint/>
<site name="replicated"/>
<geom name="replicated" size="1"/>
</body>
</replicate>
</worldbody>
<contact>
<pair geom1="floor" geom2="replicated" condim="1"/>
<exclude body1="world" body2="replicated" />
</contact>
<tendon>
<spatial>
<site site="world"/>
<site site="replicated"/>
</spatial>
</tendon>
<sensor>
<framepos name="sensor" objtype="site" objname="replicated" reftype="site" refname="world"/>
</sensor>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, testing::NotNull()) << error.data();
EXPECT_THAT(m->nbody, 3);
EXPECT_THAT(m->ngeom, 3);
EXPECT_THAT(m->npair, 2);
EXPECT_THAT(m->nexclude, 2);
EXPECT_THAT(m->ntendon, 2);
EXPECT_THAT(m->nsensor, 2);
mj_deleteModel(m);
}
TEST_F(XMLReaderTest, ParseReplicateDefaultPropagate) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<default class="body">
<geom type="capsule"/>
</default>
</default>
<worldbody>
<replicate count="2" euler="0 0 16.36" sep="-">
<body name="torso" pos="-5 0 1.282" childclass="body">
<body name="head" pos="0 0 .19">
<geom name="head" type="sphere" size=".09"/>
</body>
</body>
</replicate>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjSpec* spec = ParseSpecFromString(xml, error.data(), error.size());
EXPECT_THAT(spec, NotNull()) << error.data();
mjsBody* torso = mjs_findBody(spec, "torso-0");
EXPECT_THAT(torso, NotNull());
mjsDefault* def = mjs_getDefault(torso->element);
EXPECT_THAT(def, NotNull());
EXPECT_THAT(def->geom->type, mjGEOM_CAPSULE);
mjsBody* head = mjs_findBody(spec, "head-0");
EXPECT_THAT(head, NotNull());
def = mjs_getDefault(head->element);
EXPECT_THAT(def, NotNull());
EXPECT_THAT(def->geom->type, mjGEOM_CAPSULE);
mjs_deleteSpec(spec);
}
// ----------------------- test camera parsing ---------------------------------
TEST_F(XMLReaderTest, CameraInvalidFovyAndSensorsize) {