// 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 engine/{engine_support.c and engine_core_util.c} #include "src/engine/engine_support.h" #include #include #include #include #include #include #include #include #include "test/fixture.h" namespace mujoco { namespace { using ::std::vector; using ::testing::ContainsRegex; // NOLINT using ::testing::Eq; using ::testing::MatchesRegex; using ::testing::Ne; using ::testing::NotNull; using ::testing::Pointwise; using Name2idTest = MujocoTest; static constexpr char name2idTestingModel[] = R"( )"; TEST_F(Name2idTest, FindIds) { char error[1024]; mjModel* model = LoadModelFromString(name2idTestingModel, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "world"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body1"), 1); EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body2"), 2); EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom2"), 1); EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint2"), 1); EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "mesh1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "light1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "camera1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "site2"), 1); EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "material1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "texture1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "tendon1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "actuator1"), 0); EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "sensor1"), 0); mj_deleteModel(model); } TEST_F(Name2idTest, MissingIds) { char error[1024]; mjModel* model = LoadModelFromString(name2idTestingModel, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "abody3"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "abody2_geom2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint3"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "amesh2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "alight2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "acamera2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "asite3"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "amaterial2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "atexture2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "atendon2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "aactuator2"), -1); EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "asensor2"), -1); mj_deleteModel(model); } TEST_F(Name2idTest, EmptyIds) { char error[1024]; mjModel* model = LoadModelFromString(name2idTestingModel, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, ""), -1); mj_deleteModel(model); } TEST_F(Name2idTest, Namespaces) { char error[1024]; mjModel* model = LoadModelFromString(name2idTestingModel, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "camera1"), 3); mj_deleteModel(model); } using VersionTest = MujocoTest; TEST_F(VersionTest, MjVersion) { EXPECT_EQ(mj_version(), mjVERSION_HEADER); } TEST_F(VersionTest, MjVersionString) { #if GTEST_USES_SIMPLE_RE == 1 auto regex_matcher = ContainsRegex("^\\d+\\.\\d+\\.\\d+"); #else auto regex_matcher = MatchesRegex("^[0-9]+\\.[0-9]+\\.[0-9]+(-[0-9a-z]+)?$"); #endif EXPECT_THAT(std::string(mj_versionString()), regex_matcher); } using SupportTest = MujocoTest; // utility: generate two random quaternions with a given angle difference void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) { // make distribution using seed std::mt19937_64 rng; rng.seed(seed); std::normal_distribution dist(0, 1); // sample qa = qb for (int i=0; i < 4; i++) { qa[i] = qb[i] = dist(rng); } mju_normalize4(qa); mju_normalize4(qb); // integrate qb in random direction by angle mjtNum dir[3]; for (int i=0; i < 3; i++) { dir[i] = dist(rng); } mju_normalize3(dir); mju_quatIntegrate(qb, dir, angle); } static constexpr char ballJointModel[] = R"( )"; TEST_F(SupportTest, DifferentiatePosSubQuat) { const mjtNum eps = 1e-12; // epsilon for float comparison char error[1024]; mjModel* model = LoadModelFromString(ballJointModel, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; int seed = 1; for (mjtNum angle : {0.0, 1e-5, 1e-2}) { for (mjtNum dt : {1e-6, 1e-3, 1e-1}) { // random quaternion pair with given angle difference mjtNum qpos1[4], qpos2[4]; randomQuatPair(qpos1, qpos2, angle, seed++); // get velocity given timestep mjtNum qvel[3]; mj_differentiatePos(model, qvel, dt, qpos1, qpos2); // equivalent computation mjtNum qneg[4], qdif[4], qvel_expect[3]; mju_negQuat(qneg, qpos1); mju_mulQuat(qdif, qneg, qpos2); mju_quat2Vel(qvel_expect, qdif, dt); // expect numerical equality EXPECT_THAT(AsVector(qvel, 3), Pointwise(MjNear(eps, 1e-3), qvel_expect)); } } mj_deleteModel(model); } static const char* const kDefaultModel = "testdata/model.xml"; using StateTest = MujocoTest; TEST_F(StateTest, GetSetStateStepEqual) { const std::string xml_path = GetTestDataFilePath(kDefaultModel); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); mjData* data = mj_makeData(model); // make distribution using seed std::mt19937_64 rng; rng.seed(3); std::normal_distribution dist(0, .01); // set controls and applied joint forces to random values for (int i=0; i < model->nu; i++) data->ctrl[i] = dist(rng); for (int i=0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng); for (int i=0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0; // take one step mj_step(model, data); int signature = mjSTATE_INTEGRATION; int size = mj_stateSize(model, signature); // save the initial state and step vector state0a(size); mj_getState(model, data, state0a.data(), signature); // get the initial state, expect equality vector state0b(size); mj_getState(model, data, state0b.data(), signature); EXPECT_EQ(state0a, state0b); // take one step mj_step(model, data); // save the resulting state vector state1a(size); mj_getState(model, data, state1a.data(), signature); // expect the state to be different after stepping EXPECT_THAT(state0a, testing::Ne(state1a)); // reset to the saved state, step again, get the resulting state mj_setState(model, data, state0a.data(), signature); mj_step(model, data); vector state1b(size); mj_getState(model, data, state1b.data(), signature); // expect the state to be the same after re-stepping EXPECT_EQ(state1a, state1b); mj_deleteData(data); mj_deleteModel(model); } TEST_F(StateTest, GetSetStateDelay) { static constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); // verify history buffer exists: nhistory = 2 + 2*5 = 12 EXPECT_EQ(model->nhistory, 12); // [user, cursor, times(5), values(5)] // state size should include history buffer int size = mj_stateSize(model, mjSTATE_HISTORY); EXPECT_EQ(size, model->nhistory); // step to populate history buffer data->ctrl[0] = 1.0; mj_step(model, data); data->ctrl[0] = 2.0; mj_step(model, data); // get history state vector history_state(size); mj_getState(model, data, history_state.data(), mjSTATE_HISTORY); // modify the history buffer manually (value at index 7 = 2+5 = after times) data->history[7] = 99.0; // first value // set history state back - should restore original mj_setState(model, data, history_state.data(), mjSTATE_HISTORY); // verify restoration EXPECT_NE(data->history[7], 99.0); mj_deleteData(data); mj_deleteModel(model); } TEST_F(StateTest, CopyState) { const std::string xml_path = GetTestDataFilePath(kDefaultModel); mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); mjData* src = mj_makeData(m); mjData* dst = mj_makeData(m); // init both datas to default mj_resetData(m, src); mj_resetData(m, dst); // modify d_src src->time = 1.23; for (int i=0; i < m->nq; ++i) src->qpos[i] = i*0.1; for (int i=0; i < m->nv; ++i) src->qvel[i] = i*0.2; for (int i=0; i < m->na; ++i) src->act[i] = i*0.3; for (int i=0; i < m->nu; ++i) src->ctrl[i] = i*0.4; for (int i=0; i < m->nhistory; ++i) src->history[i] = i*0.5; for (int i=0; i < m->neq; ++i) src->eq_active[i] = 1 - m->eq_active0[i]; // check that states differ EXPECT_NE(src->time, dst->time); EXPECT_THAT(AsVector(src->qpos, m->nq), Ne(AsVector(dst->qpos, m->nq))); EXPECT_THAT(AsVector(src->ctrl, m->nu), Ne(AsVector(dst->ctrl, m->nu))); // copy state with signature int signature = mjSTATE_FULLPHYSICS | mjSTATE_EQ_ACTIVE; mj_copyState(m, src, dst, signature); // check copied components EXPECT_EQ(dst->time, src->time); EXPECT_EQ(AsVector(dst->qpos, m->nq), AsVector(src->qpos, m->nq)); EXPECT_EQ(AsVector(dst->qvel, m->nv), AsVector(src->qvel, m->nv)); EXPECT_EQ(AsVector(dst->act, m->na), AsVector(src->act, m->na)); EXPECT_EQ(AsVector(dst->history, m->nhistory), AsVector(src->history, m->nhistory)); EXPECT_EQ(AsVector(dst->eq_active, m->neq), AsVector(src->eq_active, m->neq)); // check non-copied components (CTRL not in signature) EXPECT_THAT(AsVector(dst->ctrl, m->nu), Ne(AsVector(src->ctrl, m->nu))); EXPECT_EQ(AsVector(dst->ctrl, m->nu), vector(m->nu, 0.0)); mj_deleteData(src); mj_deleteData(dst); mj_deleteModel(m); } TEST_F(StateTest, ExtractState) { const std::string xml_path = GetTestDataFilePath(kDefaultModel); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); mjData* data = mj_makeData(model); // make distribution using seed std::mt19937_64 rng; rng.seed(3); std::normal_distribution dist(0, .01); // set controls and applied joint forces to random values for (int i=0; i < model->nu; i++) data->ctrl[i] = dist(rng); for (int i=0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng); for (int i=0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0; // take one step mj_step(model, data); // take a state that will be used as src int srcsig = mjSTATE_TIME | mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_CTRL | mjSTATE_HISTORY; int srcsize = mj_stateSize(model, srcsig); vector srcstate(srcsize); mj_getState(model, data, srcstate.data(), srcsig); // extract a subset consisting of only a single bit in srcsig int dstsig1 = mjSTATE_CTRL; int dstsize1 = mj_stateSize(model, dstsig1); EXPECT_LT(dstsize1, srcsize); EXPECT_EQ(dstsize1, model->nu); vector dststate1(dstsize1); mj_extractState(model, srcstate.data(), srcsig, dststate1.data(), dstsig1); EXPECT_EQ(dststate1, AsVector(data->ctrl, model->nu)); // extract a subset consisting of multiple non-consecutive bits in srcsig int dstsig2 = mjSTATE_QPOS | mjSTATE_CTRL; int dstsize2 = mj_stateSize(model, dstsig2); EXPECT_LT(dstsize2, srcsize); EXPECT_EQ(dstsize2, model->nq + model->nu); vector dststate2(dstsize2); mj_extractState(model, srcstate.data(), srcsig, dststate2.data(), dstsig2); EXPECT_EQ(AsVector(dststate2.data(), model->nq), AsVector(data->qpos, model->nq)); EXPECT_EQ(AsVector(dststate2.data() + model->nq, model->nu), AsVector(data->ctrl, model->nu)); // extract history state int dstsig3 = mjSTATE_HISTORY; int dstsize3 = mj_stateSize(model, dstsig3); EXPECT_EQ(dstsize3, model->nhistory); vector dststate3(dstsize3); mj_extractState(model, srcstate.data(), srcsig, dststate3.data(), dstsig3); EXPECT_EQ(dststate3, AsVector(data->history, model->nhistory)); // test that an error is correctly raised if dstsig is not a subset of srcsig static int error_count; static char last_error_msg[128]; error_count = 0; last_error_msg[0] = '\0'; auto* error_handler = +[](const char* msg) { std::strncpy(last_error_msg, msg, sizeof(last_error_msg)); ++error_count; }; auto* old_mju_user_error = mju_user_error; mju_user_error = error_handler; mj_extractState(model, nullptr, srcsig, nullptr, mjSTATE_QFRC_APPLIED); EXPECT_EQ(error_count, 1); EXPECT_EQ(std::string_view(last_error_msg), "mj_extractState: dstsig is not a subset of srcsig"); mj_extractState(model, nullptr, -1, nullptr, mjSTATE_QFRC_APPLIED); EXPECT_EQ(error_count, 2); EXPECT_EQ(std::string_view(last_error_msg), "mj_extractState: invalid srcsig -1 < 0"); mju_user_error = old_mju_user_error; mj_deleteData(data); mj_deleteModel(model); } using InertiaTest = MujocoTest; static const char* const kInertiaPath = "engine/testdata/inertia.xml"; TEST_F(InertiaTest, AddMdenseSameAsSparse) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024]; mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; int nv = m->nv; mjData* d = mj_makeData(m); mj_step(m, d); // dense matrix, all values are 3.0 vector dst_dense(nv * nv, 3.0); // sparse matrix, all values are 3.0 vector dst_sparse(nv * nv, 3.0); vector rownnz(nv, nv); vector rowadr(nv, 0); vector colind(nv * nv, 0); // set sparse structure for (int i = 0; i < nv; i++) { rowadr[i] = i * nv; for (int j = 0; j < nv; j++) { colind[rowadr[i] + j] = j; } } // sparse addM mj_addM(m, d, dst_sparse.data(), rownnz.data(), rowadr.data(), colind.data()); // dense addM mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr); // dense comparison (lower triangle) for (int i=0; i < nv; i++) { for (int j=0; j < nv; j++) { EXPECT_EQ(dst_dense[i*nv+j], dst_sparse[i*nv+j]); } } // clean up mj_deleteData(d); mj_deleteModel(m); } TEST_F(InertiaTest, mulM) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024]; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error; int nv = model->nv; mjData* data = mj_makeData(model); mj_forward(model, data); // dense M matrix vector Mdense(nv*nv); mju_sym2dense(Mdense.data(), data->M, nv, model->M_rownnz, model->M_rowadr, model->M_colind); // arbitrary RHS vector vector vec(nv); for (int i=0; i < nv; i++) vec[i] = vec[i] = 20 + 30*i; // multiply directly vector res1(nv, 0); mju_mulMatVec(res1.data(), Mdense.data(), vec.data(), nv, nv); // multiply with mj_mulM vector res2(nv, 0); mj_mulM(model, data, res2.data(), vec.data()); // expect vectors to match to floating point precision EXPECT_THAT(res1, Pointwise(MjNear(1e-10, 0.1), res2)); mj_deleteData(data); mj_deleteModel(model); } TEST_F(InertiaTest, mulM2) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024]; mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error; int nv = model->nv; mjData* data = mj_makeData(model); mj_forward(model, data); // arbitrary RHS vector vector vec(nv); for (int i=0; i < nv; i++) vec[i] = .2 + .3*i; // multiply sqrtMvec = M^1/2 * vec vector sqrtMvec(nv); mj_mulM2(model, data, sqrtMvec.data(), vec.data()); // multiply Mvec = M * vec vector Mvec(nv); mj_mulM(model, data, Mvec.data(), vec.data()); // compute vec' * M * vec in two different ways, expect them to match mjtNum sqrtMvec2 = mju_dot(sqrtMvec.data(), sqrtMvec.data(), nv); mjtNum vecMvec = mju_dot(vec.data(), Mvec.data(), nv); EXPECT_MJTNUM_EQ(sqrtMvec2, vecMvec); mj_deleteData(data); mj_deleteModel(model); } TEST_F(InertiaTest, FullM) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024]; mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; int nv = m->nv; // forward dynamics, populate M and qLD mjData* d = mj_makeData(m); mj_forward(m, d); // get dense mass matrix from M using mj_fullM vector M(nv * nv); mj_fullM(m, d, M.data()); // get dense mass matrix from M using mju_sparse2dense vector M_CSR(nv * nv); mju_sparse2dense(M_CSR.data(), d->M, nv, nv, m->M_rownnz, m->M_rowadr, m->M_colind); // expect lower triangles to match exactly for (int i = 0; i < nv; ++i) { for (int j = 0; j <= i; ++j) { EXPECT_EQ(M[i * nv + j], M_CSR[i * nv + j]); } } // get dense LTDL factor (D on the diagonal) vector LD(nv * nv); mju_sparse2dense(LD.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr, m->M_colind); // extract L and D from LD vector L = LD; vector D(nv * nv, 0.0); for (int i = 0; i < nv; i++) { D[i * nv + i] = LD[i * nv + i]; L[i * nv + i] = 1.0; } // compute DL = D * L vector DL(nv * nv, 0.0); mju_mulMatMat(DL.data(), D.data(), L.data(), nv, nv, nv); // compute the triple product P = L^T * D * L vector P(nv * nv, 0.0); mju_mulMatTMat(P.data(), L.data(), DL.data(), nv, nv, nv); // expect M and P to match to high precision EXPECT_THAT(M, Pointwise(MjNear(1e-10, 1e-4), P)); mj_deleteData(d); mj_deleteModel(m); } static constexpr char GeomDistanceTestingModel1[] = R"( )"; static constexpr char GeomDistanceTestingModel2[] = R"( )"; TEST_F(SupportTest, GeomDistance) { char error[1024]; mjModel* model = LoadModelFromString(GeomDistanceTestingModel1, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); mj_kinematics(model, data); // plane-sphere, distmax too small mjtNum distmax = 0.5; EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, nullptr), 0.5); mjtNum fromto[6]; EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, fromto), 0.5); EXPECT_THAT(fromto, Pointwise(Eq(), vector{0, 0, 0, 0, 0, 0})); // plane-sphere distmax = 1.0; EXPECT_THAT(mj_geomDistance(model, data, 0, 1, 1.0, fromto), MjNear(0.8, 1e-12, 1e-5)); mjtNum eps = 1e-12; EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), vector{0, 0, 0, 0, 0, 0.8})); // sphere-plane EXPECT_THAT(mj_geomDistance(model, data, 1, 0, 1.0, fromto), MjNear(0.8, 1e-12, 1e-5)); EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), vector{0, 0, 0.8, 0, 0, 0})); // sphere-sphere EXPECT_THAT(mj_geomDistance(model, data, 1, 2, 1.0, fromto), MjNear(0.5, 1e-12, 1e-5)); EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), vector{.2, 0, 1, .7, 0, 1})); // sphere-sphere, flipped order EXPECT_THAT(mj_geomDistance(model, data, 2, 1, 1.0, fromto), MjNear(0.5, 1e-12, 1e-5)); EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), vector{.7, 0, 1, .2, 0, 1})); // mesh-sphere (close distmax) distmax = 0.701; eps = model->opt.ccd_tolerance; EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto), MjNear(0.7, eps, eps*100)); EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps*100), vector{0, 0, .1, 0, 0, .8})); // mesh-sphere (far distmax) distmax = 1.0; EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto), MjNear(0.7, eps, eps*100)); EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps*100), vector{0, 0, .1, 0, 0, .8})); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SupportTest, GeomDistanceFromToFlipped) { mjtNum distmax = 10.0; char error[1024]; mjModel* model = LoadModelFromString(GeomDistanceTestingModel2, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); mj_kinematics(model, data); mjtNum fromto01[6]; mjtNum fromto10[6]; for (int flag : {0, (int)mjDSBL_NATIVECCD}) { model->opt.disableflags = flag; mj_geomDistance(model, data, 0, 1, distmax, fromto01); mj_geomDistance(model, data, 1, 0, distmax, fromto10); mjtNum fromto10flipped[6] = {fromto10[3], fromto10[4], fromto10[5], fromto10[0], fromto10[1], fromto10[2]}; EXPECT_THAT(AsVector(fromto10flipped, 6), Pointwise(MjNear(1.0e-12, 1e-5), fromto01)); } mj_deleteData(data); mj_deleteModel(model); } static constexpr char kSetKeyframeTestingModel[] = R"( )"; TEST_F(SupportTest, SetKeyframe) { char error[1024]; mjModel* model = LoadModelFromString(kSetKeyframeTestingModel, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); data->ctrl[0] = 1; while (data->time < 1) { mj_step(model, data); } mj_setKeyframe(model, data, 1); EXPECT_EQ(data->time, model->key_time[1]); EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 1]); EXPECT_EQ(data->qpos[0], model->key_qpos[model->nq * 1]); EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 1]); EXPECT_EQ(data->act[0], model->key_act[model->na * 1]); mj_step(model, data); mj_setKeyframe(model, data, 0); EXPECT_EQ(data->time, model->key_time[0]); EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 0]); EXPECT_EQ(data->qpos[0], model->key_qpos[model->nq * 0]); EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 0]); EXPECT_EQ(data->act[0], model->key_act[model->na * 0]); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SupportTest, ContactSensorDim) { int dataSpec = 1 << mjCONDATA_FOUND | 1 << mjCONDATA_FORCE | 1 << mjCONDATA_DIST | 1 << mjCONDATA_POS | 1 << mjCONDATA_TANGENT; EXPECT_EQ(mju_condataSize(dataSpec), 1+3+1+3+3); } // ------------------------------ ctrl delays -------------------------------- TEST_F(SupportTest, ReadCtrlNoDelay) { static constexpr char xml[] = R"( )"; mjModel* model = LoadModelFromString(xml); ASSERT_THAT(model, NotNull()); mjData* data = mj_makeData(model); // no delay: should return current ctrl value data->ctrl[0] = 42.0; EXPECT_EQ(mj_readCtrl(model, data, 0, data->time, /*order=*/0), 42.0); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SupportTest, ReadCtrlWithDelay) { static constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); // model should have delay configured // delay = 0.03 seconds, timestep = 0.01, so ndelay = ceil(0.03/0.01) = 3 EXPECT_EQ(model->actuator_history[0], 3); EXPECT_NEAR(model->actuator_delay[0], 0.03, 1e-7); EXPECT_GE(model->actuator_historyadr[0], 0); // initially, buffer should be filled with constant value (from init) // reading at current time should return the init value mjtNum val = mj_readCtrl(model, data, 0, data->time, /*order=*/0); EXPECT_EQ(val, data->ctrl[0]); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SupportTest, InitCtrlDelay) { static constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); // verify nhistory EXPECT_EQ(model->actuator_history[0], 3); // initialize with custom times and values // buffer stores: time 0.0 -> value 1.0, time 0.01 -> value 2.0, time 0.02 -> value 3.0 mjtNum times[3] = {0.0, 0.01, 0.02}; mjtNum values[3] = {1.0, 2.0, 3.0}; mj_initCtrlHistory(model, data, 0, times, values); // mj_readCtrl now auto-subtracts delay: lookup_time = time - delay // delay = 0.02, so: // time=0.04 -> lookup at 0.02 -> value 3.0 // time=0.03 -> lookup at 0.01 -> value 2.0 // time=0.02 -> lookup at 0.00 -> value 1.0 mjtNum val = mj_readCtrl(model, data, 0, 0.04, /*order=*/0); EXPECT_EQ(val, 3.0); val = mj_readCtrl(model, data, 0, 0.03, /*order=*/0); EXPECT_EQ(val, 2.0); val = mj_readCtrl(model, data, 0, 0.02, /*order=*/0); EXPECT_EQ(val, 1.0); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SupportTest, InitCtrlDelayNullTimes) { static constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); // get existing times from buffer int adr = model->actuator_historyadr[0]; mjtNum* buf = data->history + adr; mjtNum existing_times[3] = {buf[2], buf[3], buf[4]}; // initialize with NULL times (use existing) and new values mjtNum values[3] = {10.0, 20.0, 30.0}; mj_initCtrlHistory(model, data, 0, nullptr, values); // verify times are unchanged EXPECT_EQ(buf[2], existing_times[0]); EXPECT_EQ(buf[3], existing_times[1]); EXPECT_EQ(buf[4], existing_times[2]); // verify values are updated EXPECT_EQ(buf[5], 10.0); EXPECT_EQ(buf[6], 20.0); EXPECT_EQ(buf[7], 30.0); mj_deleteData(data); mj_deleteModel(model); } TEST_F(SupportTest, InitSensorDelay) { static constexpr char xml[] = R"( )"; char error[1024]; mjModel* model = LoadModelFromString(xml, error, sizeof(error)); ASSERT_THAT(model, NotNull()) << error; mjData* data = mj_makeData(model); // verify nsample for sensor EXPECT_EQ(model->sensor_history[0], 3); // initialize with custom times and values, phase=0 // buffer stores: time 0.0 -> value 0.5, time 0.01 -> value 0.6, time 0.02 -> value 0.7 mjtNum times[3] = {0.0, 0.01, 0.02}; mjtNum values[3] = {0.5, 0.6, 0.7}; mj_initSensorHistory(model, data, 0, times, values, /*phase=*/0.0); // mj_readSensor now auto-subtracts delay: lookup_time = time - delay // delay = 0.02, so: // time=0.04 -> lookup at 0.02 -> value 0.7 // time=0.03 -> lookup at 0.01 -> value 0.6 mjtNum result = 0; const mjtNum* ptr = mj_readSensor(model, data, 0, 0.04, &result, /*order=*/0); mjtNum val = ptr ? *ptr : result; EXPECT_NEAR(val, 0.7, 1e-6); ptr = mj_readSensor(model, data, 0, 0.03, &result, /*order=*/0); val = ptr ? *ptr : result; EXPECT_NEAR(val, 0.6, 1e-6); mj_deleteData(data); mj_deleteModel(model); } } // namespace } // namespace mujoco