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

Add sleep related data structures

Browse Source 
PiperOrigin-RevId: 829055431
Change-Id: I1ccbd77a57044a754ae7db611b4c2c0010fbda53
This commit is contained in:
Yuval Tassa
2025-11-06 12:09:59 -08:00
committed by Copybara-Service
parent 3080e3424f
commit 252a0d73df
37 changed files with 1907 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/CMakeLists.txt
+4
View File
@@ -51,6 +51,10 @@ mujoco_test(engine_ray_test)
mujoco_test(engine_sensor_test)
mujoco_test(engine_setconst_test)
mujoco_test(engine_sleep_test)
mujoco_test(engine_solver_test)
mujoco_test(engine_sort_test)
test/engine/engine_core_smooth_test.cc
+1 -1
View File
@@ -79,7 +79,7 @@ TEST_F(CoreSmoothTest, MjDataWorldBodyValuesAreInitialized) {
EXPECT_THAT(values, ElementsAre(1, 0, 0, 0)) << #name; \
} else if constexpr (EndsWith(#name, "mat")) { \
EXPECT_THAT(values, ElementsAre(1, 0, 0, 0, 1, 0, 0, 0, 1)) << #name; \
} else { \
} else if constexpr (std::string_view(#type) == "mjtNum") { \
EXPECT_THAT(values, Each(Eq(0))) << #name; \
} \
}
test/engine/engine_setconst_test.cc
+479
View File
@@ -0,0 +1,479 @@
// Copyright 2025 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 engine/engine_setconst.c.
#include <cstddef>
#include <string>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::std::string;
using ::testing::DoubleNear;
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::NotNull;
using SetConstTest = MujocoTest;
TEST_F(SetConstTest, AwakeActuatedJoint) {
string xml = R"(
<mujoco>
<worldbody>
<body name="B1" sleep="POLICY1">
<joint name="J1" type="slide"/>
<geom size=".1"/>
</body>
<body name="B2" sleep="POLICY2">
<joint name="J2" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<motor joint="J1"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* m;
string sleep[] = {"auto", "never", "allowed", "init"};
int tsp0[] = {mjSLEEP_AUTO_NEVER, mjSLEEP_NEVER, mjSLEEP_ALLOWED,
mjSLEEP_INIT};
int tsp1[] = {mjSLEEP_AUTO_ALLOWED, mjSLEEP_NEVER, mjSLEEP_ALLOWED,
mjSLEEP_INIT};
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
string xml_copy = xml;
size_t pos1 = xml_copy.find("POLICY1");
xml_copy.replace(pos1, 7, sleep[i]);
size_t pos2 = xml_copy.find("POLICY2");
xml_copy.replace(pos2, 7, sleep[j]);
m = LoadModelFromString(xml_copy.c_str(), error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
EXPECT_EQ(m->tree_sleep_policy[0], tsp0[i]);
EXPECT_EQ(m->tree_sleep_policy[1], tsp1[j]);
mj_deleteModel(m);
}
}
}
TEST_F(SetConstTest, AwakeActuatedSite) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint name="J1" type="slide"/>
<geom size=".1"/>
<site name="S1"/>
</body>
<body name="B2">
<joint name="J2" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<general site="S1" gear="1 0 0 0 0 0"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
}
TEST_F(SetConstTest, AwakeActuatedBody) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint name="J1" type="slide"/>
<geom size=".1"/>
</body>
<body name="B2">
<joint name="J2" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<adhesion body="B1" ctrlrange="0 1"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
}
TEST_F(SetConstTest, AwakeActuatedTendon) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<site name="S1"/>
<body name="B1">
<joint name="J1" type="slide"/>
<geom size=".1"/>
<site name="S2"/>
</body>
<body name="B2">
<joint name="J2" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<tendon>
<spatial name="T1">
<site site="S1"/>
<site site="S2"/>
</spatial>
</tendon>
<actuator>
<motor tendon="T1"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
}
TEST_F(SetConstTest, AwakeStiffTendonMultiTree) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint name="J1" type="slide"/>
<geom size=".1"/>
<site name="S1"/>
</body>
<body name="B2">
<joint name="J2" type="slide"/>
<geom size=".1"/>
<site name="S2"/>
</body>
</worldbody>
<tendon>
<spatial name="T1" stiffness="1">
<site site="S1"/>
<site site="S2"/>
</spatial>
</tendon>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_NEVER);
mj_deleteModel(model);
}
TEST_F(SetConstTest, SleepyTendonSingleTree) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint name="J1" type="slide"/>
<geom size=".1"/>
<site name="S1"/>
<body name="B2">
<joint name="J2" type="slide"/>
<geom size=".1"/>
<site name="S2"/>
</body>
</body>
</worldbody>
<tendon>
<spatial name="T1" stiffness="1">
<site site="S1"/>
<site site="S2"/>
</spatial>
</tendon>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
}
TEST_F(SetConstTest, SleepyTendonZeroStiffness) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint name="J1" type="slide"/>
<geom size=".1"/>
<site name="S1"/>
</body>
<body name="B2">
<joint name="J2" type="slide"/>
<geom size=".1"/>
<site name="S2"/>
</body>
</worldbody>
<tendon>
<spatial name="T1" stiffness="0" damping="0">
<site site="S1"/>
<site site="S2"/>
</spatial>
</tendon>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
}
TEST_F(SetConstTest, TendonTreeId) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<!-- Static body 1: world -->
<site name="S1"/>
<!-- Static body 2: child of world -->
<body name="B_static">
<site name="S2"/>
</body>
<!-- Tree 1 -->
<body name="B1_1">
<joint name="J1_1" type="slide"/>
<geom size=".1"/>
<site name="S3"/>
<body name="B1_2">
<joint name="J1_2" type="slide"/>
<geom size=".1"/>
<site name="S4"/>
</body>
</body>
<!-- Tree 2 -->
<body name="B2_1">
<joint name="J2_1" type="slide"/>
<geom size=".1"/>
<site name="S5"/>
</body>
<!-- Tree 3 -->
<body name="B3_1">
<joint name="J3_1" type="slide"/>
<geom size=".1"/>
<site name="S6"/>
</body>
</worldbody>
<tendon>
<!-- Tendon 1: Between static bodies -->
<spatial name="T_static">
<site site="S1"/>
<site site="S2"/>
</spatial>
<!-- Tendon 2: Within Tree 1 -->
<spatial name="T_tree1">
<site site="S3"/>
<site site="S4"/>
</spatial>
<!-- Tendon 3: Between Tree 1 and Tree 2 -->
<spatial name="T_intertree12">
<site site="S4"/>
<site site="S5"/>
</spatial>
<!-- Tendon 4: Between Tree 1, 2 and 3 -->
<spatial name="T_intertree123">
<site site="S4"/>
<site site="S5"/>
<site site="S6"/>
</spatial>
</tendon>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int t_static_id = mj_name2id(model, mjOBJ_TENDON, "T_static");
int t_tree1_id = mj_name2id(model, mjOBJ_TENDON, "T_tree1");
int t_intertree12_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree12");
int t_intertree123_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree123");
int b1_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B1_1")];
int b2_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B2_1")];
// Tendon 1: Not associated with any tree
EXPECT_EQ(model->tendon_treenum[t_static_id], 0);
EXPECT_EQ(model->tendon_treeid[2*t_static_id], -1);
EXPECT_EQ(model->tendon_treeid[2*t_static_id+1], -1);
// Tendon 2: Should be in Tree 1
EXPECT_EQ(model->tendon_treenum[t_tree1_id], 1);
EXPECT_EQ(model->tendon_treeid[2*t_tree1_id], b1_1_treeid);
EXPECT_EQ(model->tendon_treeid[2*t_tree1_id+1], -1);
EXPECT_GE(model->tendon_treeid[2*t_tree1_id], 0);
// Tendon 3: Spans two trees (Tree 1 and Tree 2)
EXPECT_EQ(model->tendon_treenum[t_intertree12_id], 2);
EXPECT_EQ(model->tendon_treeid[2*t_intertree12_id], b1_1_treeid);
EXPECT_EQ(model->tendon_treeid[2*t_intertree12_id+1], b2_1_treeid);
// Tendon 4: Spans three trees (Tree 1, 2 and 3)
EXPECT_EQ(model->tendon_treenum[t_intertree123_id], 3);
EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id], b1_1_treeid);
EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id+1], b2_1_treeid);
// The third tree ID is not stored in tendon_treeid
mj_deleteModel(model);
}
TEST_F(SetConstTest, SleepingNotAllowed) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<!-- Tree 0 -->
<body name="B1_1">
<joint name="J1_1" type="slide"/>
<geom size=".1"/>
<site name="S3"/>
<body name="B1_2">
<joint name="J1_2" type="slide"/>
<geom size=".1"/>
<site name="S4"/>
</body>
</body>
<!-- Tree 1: forbidden user sleep override -->
<body name="B2_1" sleep="allowed">
<joint name="J2_1" type="slide"/>
<geom size=".1"/>
<site name="S5"/>
</body>
<!-- Tree 2 -->
<body name="B3_1">
<joint name="J3_1" type="slide"/>
<geom size=".1"/>
<site name="S6"/>
</body>
</worldbody>
<tendon>
<!-- Tendon 0: Between Tree 0, 1 and 2 -->
<spatial name="T_intertree123">
<site site="S4"/>
<site site="S5"/>
<site site="S6"/>
</spatial>
</tendon>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
EXPECT_THAT(model, IsNull()) << error;
EXPECT_THAT(string(error), HasSubstr(
"tree 1 connected to tendon 0 which spans more than 2 trees, "
"sleeping not allowed"));
}
TEST_F(SetConstTest, DofLength) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint name="S1" type="slide"/>
<geom size="2"/>
</body>
<body name="B2">
<joint name="H1" type="hinge"/>
<geom size="3"/>
</body>
<body name="B3">
<joint name="BA1" type="ball"/>
<geom size="4"/>
</body>
<body name="B4">
<joint name="F1" type="free"/>
<geom size="5"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjtNum tol = 1e-5;
// B1: Slider
EXPECT_EQ(model->dof_length[0], 1);
// B2: Hinge
EXPECT_THAT(model->dof_length[1], DoubleNear(3, tol));
// B3: Ball
EXPECT_THAT(model->dof_length[2], DoubleNear(4, tol));
EXPECT_THAT(model->dof_length[3], DoubleNear(4, tol));
EXPECT_THAT(model->dof_length[4], DoubleNear(4, tol));
// B4: Free
EXPECT_EQ(model->dof_length[5], 1);
EXPECT_EQ(model->dof_length[6], 1);
EXPECT_EQ(model->dof_length[7], 1);
EXPECT_THAT(model->dof_length[8], DoubleNear(5, tol));
EXPECT_THAT(model->dof_length[9], DoubleNear(5, tol));
EXPECT_THAT(model->dof_length[10], DoubleNear(5, tol));
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_sleep_test.cc
+181
View File
@@ -0,0 +1,181 @@
// Copyright 2025 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 engine/engine_sleep.c.
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_sleep.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::ElementsAre;
using ::testing::NotNull;
using SleepTest = MujocoTest;
static constexpr char kSimple[] = R"(
<mujoco>
<option>
<flag constraint="disable" contact="disable"/>
</option>
<default>
<geom size="1"/>
</default>
<worldbody>
<body>
<joint type="ball"/>
<geom/>
</body>
<body>
<geom/>
</body>
<geom/>
<body>
<joint/>
<geom/>
<geom/>
<body pos="1 0 0">
<joint/>
<geom/>
</body>
</body>
</worldbody>
</mujoco>
)";
static constexpr int kAwake = -(1+mjMINAWAKE);
TEST_F(SleepTest, MjSleepUpdate) {
char error[1024];
mjModel* m = LoadModelFromString(kSimple, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
// ntree = 2, nbody = 5, nv = 5, njnt = 3, ngeom = 6
// body 0: world, 1 geom
// body 1: 1 ball join, 3 dofs, 1 geom
// body 2: no joint, 1 geom
// body 3: hinge joint, 1 dof, 2 geoms
// body 4: child of body 2, hinge joint, 1 dof, 1 geom
EXPECT_EQ(m->ntree, 2);
EXPECT_EQ(m->nbody, 5);
EXPECT_EQ(m->nv, 5);
EXPECT_EQ(m->njnt, 3);
EXPECT_EQ(m->ngeom, 6);
EXPECT_THAT(AsVector(m->body_treeid, m->nbody),
ElementsAre(-1, 0, -1, 1, 1));
EXPECT_THAT(AsVector(m->dof_bodyid, m->nv),
ElementsAre(1, 1, 1, 3, 4));
EXPECT_THAT(AsVector(m->geom_bodyid, m->ngeom),
ElementsAre(0, 1, 2, 3, 3, 4));
EXPECT_THAT(AsVector(m->jnt_bodyid, m->njnt),
ElementsAre(1, 3, 4));
// Test Case 1: Initial state
EXPECT_THAT(AsVector(d->tree_asleep, m->ntree),
ElementsAre(kAwake, kAwake));
EXPECT_EQ(d->ntree_awake, 2);
EXPECT_EQ(d->nv_awake, 5);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre(0, 1, 2, 3, 4));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(1, 1));
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC,
mjS_AWAKE,
mjS_STATIC,
mjS_AWAKE,
mjS_AWAKE));
// Test Case 2: Call mj_sleepUpdate, expect no changes
mj_updateSleep(m, d);
EXPECT_EQ(d->ntree_awake, 2);
EXPECT_EQ(d->nv_awake, 5);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre(0, 1, 2, 3, 4));
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC,
mjS_AWAKE,
mjS_STATIC,
mjS_AWAKE,
mjS_AWAKE));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(1, 1));
// Test Case 3: Tree 0 asleep
d->tree_asleep[0] = 0; d->tree_asleep[1] = -1;
mj_updateSleep(m, d);
EXPECT_EQ(d->ntree_awake, 1);
EXPECT_EQ(d->nv_awake, 2);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre(3, 4));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(0, 1));
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC,
mjS_ASLEEP,
mjS_STATIC,
mjS_AWAKE,
mjS_AWAKE));
// Test Case 4: Tree 1 asleep
d->tree_asleep[0] = -1; d->tree_asleep[1] = 1;
mj_updateSleep(m, d);
EXPECT_EQ(d->ntree_awake, 1);
EXPECT_EQ(d->nv_awake, 3);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre(0, 1, 2));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(1, 0));
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC,
mjS_AWAKE,
mjS_STATIC,
mjS_ASLEEP,
mjS_ASLEEP));
// Test Case 5: All trees asleep
d->tree_asleep[0] = 0; d->tree_asleep[1] = 1;
mj_updateSleep(m, d);
EXPECT_EQ(d->ntree_awake, 0);
EXPECT_EQ(d->nv_awake, 0);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre());
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(0, 0));
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC,
mjS_ASLEEP,
mjS_STATIC,
mjS_ASLEEP,
mjS_ASLEEP));
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
} // namespace mujoco