// 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 #include #include #include #include #include #include "test/fixture.h" namespace mujoco { namespace { using ::std::string; using ::testing::HasSubstr; using ::testing::IsNull; using ::testing::NotNull; using SetConstTest = MujocoTest; TEST_F(SetConstTest, AwakeActuatedJoint) { string xml = R"( )"; char error[1024]; MjModelPtr 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.get(), NotNull()) << error; EXPECT_EQ(m->tree_sleep_policy[0], tsp0[i]); EXPECT_EQ(m->tree_sleep_policy[1], tsp1[j]); } } } TEST_F(SetConstTest, AwakeActuatedSite) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); } TEST_F(SetConstTest, AwakeActuatedBody) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); } TEST_F(SetConstTest, AwakeActuatedTendon) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); } TEST_F(SetConstTest, AwakeStiffTendonMultiTree) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_NEVER); } TEST_F(SetConstTest, SleepyTendonSingleTree) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED); } TEST_F(SetConstTest, SleepyTendonZeroStiffness) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); } TEST_F(SetConstTest, TendonTreeId) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; int t_static_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_static"); int t_tree1_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_tree1"); int t_intertree12_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree12"); int t_intertree123_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree123"); int b1_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B1_1")]; int b2_1_treeid = model->body_treeid[mj_name2id(model.get(), 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 } TEST_F(SetConstTest, SleepingNotAllowed) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); EXPECT_THAT(model.get(), 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"( )"; char error[1024]; MjModelPtr model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model.get(), NotNull()) << error; mjtNum tol = 1e-5; // B1: Slider EXPECT_EQ(model->dof_length[0], 1); // B2: Hinge EXPECT_NEAR(model->dof_length[1], 3, tol); // B3: Ball EXPECT_NEAR(model->dof_length[2], 4, tol); EXPECT_NEAR(model->dof_length[3], 4, tol); EXPECT_NEAR(model->dof_length[4], 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_NEAR(model->dof_length[8], 5, tol); EXPECT_NEAR(model->dof_length[9], 5, tol); EXPECT_NEAR(model->dof_length[10], 5, tol); } TEST_F(SetConstTest, BodySameframeRecomputed) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m.get(), NotNull()) << error; MjDataPtr d(mj_makeData(m.get())); int b = mj_name2id(m.get(), mjOBJ_BODY, "B1"); // initially sameframe should be BODY (ipos=0, iquat=identity) EXPECT_EQ(m->body_sameframe[b], mjSAMEFRAME_BODY); // perturb body_ipos, call mj_setConst m->body_ipos[3*b+0] = 1.0; mj_setConst(m.get(), d.get()); EXPECT_EQ(m->body_sameframe[b], mjSAMEFRAME_BODYROT); // also perturb body_iquat m->body_iquat[4*b+0] = 0.5; m->body_iquat[4*b+1] = 0.5; m->body_iquat[4*b+2] = 0.5; m->body_iquat[4*b+3] = 0.5; mj_setConst(m.get(), d.get()); EXPECT_EQ(m->body_sameframe[b], mjSAMEFRAME_NONE); // restore to identity, should go back to BODY m->body_ipos[3*b+0] = 0; m->body_iquat[4*b+0] = 1; m->body_iquat[4*b+1] = 0; m->body_iquat[4*b+2] = 0; m->body_iquat[4*b+3] = 0; mj_setConst(m.get(), d.get()); EXPECT_EQ(m->body_sameframe[b], mjSAMEFRAME_BODY); } TEST_F(SetConstTest, GeomSameframeRecomputed) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m.get(), NotNull()) << error; MjDataPtr d(mj_makeData(m.get())); int g = mj_name2id(m.get(), mjOBJ_GEOM, "G1"); // initially sameframe should be BODY EXPECT_EQ(m->geom_sameframe[g], mjSAMEFRAME_BODY); // perturb geom_pos m->geom_pos[3*g+1] = 0.5; mj_setConst(m.get(), d.get()); EXPECT_EQ(m->geom_sameframe[g], mjSAMEFRAME_BODYROT); // restore, should go back to BODY m->geom_pos[3*g+1] = 0; mj_setConst(m.get(), d.get()); EXPECT_EQ(m->geom_sameframe[g], mjSAMEFRAME_BODY); } TEST_F(SetConstTest, SiteSameframeRecomputed) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m.get(), NotNull()) << error; MjDataPtr d(mj_makeData(m.get())); int s = mj_name2id(m.get(), mjOBJ_SITE, "S1"); // initially sameframe should be BODY EXPECT_EQ(m->site_sameframe[s], mjSAMEFRAME_BODY); // perturb site_pos m->site_pos[3*s+2] = 0.3; mj_setConst(m.get(), d.get()); EXPECT_EQ(m->site_sameframe[s], mjSAMEFRAME_BODYROT); // restore m->site_pos[3*s+2] = 0; mj_setConst(m.get(), d.get()); EXPECT_EQ(m->site_sameframe[s], mjSAMEFRAME_BODY); } TEST_F(SetConstTest, SameframeKinematicsCorrect) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m.get(), NotNull()) << error; MjDataPtr d(mj_makeData(m.get())); int b = mj_name2id(m.get(), mjOBJ_BODY, "B1"); int g = mj_name2id(m.get(), mjOBJ_GEOM, "G1"); // perturb body inertial offset, breaking sameframe m->body_ipos[3*b+1] = 0.5; mj_setConst(m.get(), d.get()); EXPECT_EQ(m->body_sameframe[b], mjSAMEFRAME_BODYROT); // run forward kinematics, check that xipos != xpos mj_forward(m.get(), d.get()); EXPECT_NEAR(d->xipos[3*b+1], 0.5, MjTol(1e-10, 1e-6)); EXPECT_NEAR(d->xpos[3*b+1], 0.0, MjTol(1e-10, 1e-6)); // perturb geom_pos, check geom global position m->geom_pos[3*g+2] = 0.3; mj_setConst(m.get(), d.get()); EXPECT_NE(m->geom_sameframe[g], mjSAMEFRAME_BODY); mj_forward(m.get(), d.get()); EXPECT_NEAR(d->geom_xpos[3*g+2], 0.3, MjTol(1e-10, 1e-6)); } TEST_F(SetConstTest, SimpleBodyLostSameframeError) { constexpr char xml[] = R"( )"; char error[1024]; MjModelPtr m = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(m.get(), NotNull()) << error; MjDataPtr d(mj_makeData(m.get())); int b = mj_name2id(m.get(), mjOBJ_BODY, "B1"); // confirm body is compiled as simple EXPECT_GT(m->body_simple[b], 0); // perturb body_ipos, breaking sameframe; calling mj_setConst should fail m->body_ipos[3*b+0] = 1.0; std::string err = MjuErrorMessageFrom(mj_setConst)(m.get(), d.get()); EXPECT_THAT(err, HasSubstr("body 1 is compiled as simple but " "sameframe no longer holds")); } } // namespace } // namespace mujoco