// 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_util_solve.c. #include #include #include #include #include #include #include #include #include #include #include #include "src/engine/engine_util_misc.h" #include "test/fixture.h" namespace mujoco { namespace { using ::testing::DoubleNear; using ::testing::ElementsAre; using ::testing::ElementsAreArray; using ::testing::HasSubstr; using ::testing::NotNull; using ::testing::Pointwise; using ::testing::StrEq; using UtilMiscTest = MujocoTest; TEST_F(UtilMiscTest, PrintsMemoryWarning) { EXPECT_THAT(mju_warningText(mjWARN_CNSTRFULL, pow(2, 10)), HasSubstr("1K bytes")); EXPECT_THAT(mju_warningText(mjWARN_CNSTRFULL, pow(2, 20)), HasSubstr("1M bytes")); EXPECT_THAT(mju_warningText(mjWARN_CNSTRFULL, pow(2, 30)), HasSubstr("1G bytes")); EXPECT_THAT(mju_warningText(mjWARN_CNSTRFULL, pow(2, 40)), HasSubstr("1T bytes")); EXPECT_THAT(mju_warningText(mjWARN_CNSTRFULL, pow(2, 50)), HasSubstr("1P bytes")); EXPECT_THAT(mju_warningText(mjWARN_CNSTRFULL, pow(2, 60)), HasSubstr("1E bytes")); EXPECT_THAT(mju_warningText(mjWARN_CNSTRFULL, pow(2, 30) + 1), HasSubstr("1073741825 bytes")); } TEST_F(UtilMiscTest, Sigmoid) { // function values EXPECT_EQ(mju_sigmoid(-1), 0); EXPECT_EQ(mju_sigmoid(0), 0); EXPECT_EQ(mju_sigmoid(0.5), 0.5); EXPECT_EQ(mju_sigmoid(1), 1); EXPECT_EQ(mju_sigmoid(2), 1); // epsilon for finite-differencing const mjtNum dx = 1e-7; // derivative at 0 mjtNum dy_dx_0 = (mju_sigmoid(0 + dx) - mju_sigmoid(0)) / dx; EXPECT_THAT(dy_dx_0, DoubleNear(0, dx)); // derivative at 1 mjtNum dy_dx_1 = (mju_sigmoid(1) - mju_sigmoid(1 - dx)) / dx; EXPECT_THAT(dy_dx_1, DoubleNear(0, dx)); // derivative at 0.5 const mjtNum x = 0.5; mjtNum dy_dx_0p5 = (mju_sigmoid(x + dx) - mju_sigmoid(x - dx)) / (2*dx); mjtNum expected = 30*x*x*x*x - 60*x*x*x + 30*x*x; EXPECT_THAT(dy_dx_0p5, DoubleNear(expected, dx)); } TEST_F(UtilMiscTest, SphereWrap) { 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); // measure tendon length for keyframe 0 mj_resetDataKeyframe(model, data, 0); mj_forward(model, data); mjtNum ten_length0 = data->sensordata[0]; // measure tendon length for keyframe 1 mj_resetDataKeyframe(model, data, 1); mj_forward(model, data); mjtNum ten_length1 = data->sensordata[0]; // difference should be small mjtNum diff = ten_length1 - ten_length0; EXPECT_LT(mju_abs(diff), 1e-3); mj_deleteData(data); mj_deleteModel(model); } // compute time constant as in Millard et al. (2013) https://doi.org/10.1115/1.4023390 mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) { // clamp control mjtNum ctrlclamp = mju_clip(ctrl, 0, 1); // clamp activation mjtNum actclamp = mju_clip(act, 0, 1); mjtNum tau; if (ctrlclamp > act) { tau = prm[0] * (0.5 + 1.5*actclamp); } else { tau = prm[1] / (0.5 + 1.5*actclamp); } // filter output return (ctrlclamp-act) / mjMAX(mjMINVAL, tau); } TEST_F(UtilMiscTest, SmoothMuscleDynamics) { mjtNum prm[3] = {0.01, 0.04, 0.0}; // exact equality if tau_smooth = 0 for (mjtNum ctrl : {-0.1, 0.0, 0.4, 0.5, 1.0, 1.1}) { for (mjtNum act : {-0.1, 0.0, 0.4, 0.5, 1.0, 1.1}) { mjtNum actdot_old = muscleDynamicsMillard(ctrl, act, prm); mjtNum actdot_new = mju_muscleDynamics(ctrl, act, prm); EXPECT_EQ(actdot_new, actdot_old); } } // positive tau_smooth mjtNum tau_smooth = 0.2; prm[2] = tau_smooth; mjtNum act = 0.5; mjtNum eps = 1e-6; mjtNum ctrl = 0.4 - eps; // smaller than act by just over 0.5*tau_smooth EXPECT_EQ(muscleDynamicsMillard(ctrl, act, prm), mju_muscleDynamics(ctrl, act, prm)); ctrl = 0.6 + eps; // larger than act by just over 0.5*tau_smooth EXPECT_EQ(muscleDynamicsMillard(ctrl, act, prm), mju_muscleDynamics(ctrl, act, prm)); // right in the middle should give average of time constants mjtNum tau_act = 0.2; mjtNum tau_deact = 0.3; for (mjtNum dctrl : {0.0, 0.1, 0.2, 1.0, 1.1}) { mjtNum lower = mju_muscleDynamicsTimescale(-dctrl, tau_act, tau_deact, tau_smooth); mjtNum upper = mju_muscleDynamicsTimescale(dctrl, tau_act, tau_deact, tau_smooth); EXPECT_EQ(0.5*(upper + lower), 0.5*(tau_act + tau_deact)); } } TEST_F(UtilMiscTest, MuscleGainLength) { mjtNum lmin = 0.5; mjtNum lmax = 1.5; EXPECT_EQ(mju_muscleGainLength(0.0, lmin, lmax), 0); EXPECT_EQ(mju_muscleGainLength(0.5, lmin, lmax), 0); EXPECT_EQ(mju_muscleGainLength(0.75, lmin, lmax), 0.5); EXPECT_EQ(mju_muscleGainLength(1.0, lmin, lmax), 1); EXPECT_EQ(mju_muscleGainLength(1.25, lmin, lmax), 0.5); EXPECT_EQ(mju_muscleGainLength(1.5, lmin, lmax), 0); EXPECT_EQ(mju_muscleGainLength(2.0, lmin, lmax), 0); } TEST_F(UtilMiscTest, MjuSparseMap) { // nr = 3 // src = [[1, 2, 0], // [0, 3, 4], // [5, 0, 6]] constexpr int nr = 3; const mjtNum mat_src[] = {1, 2, 3, 4, 5, 6}; const int rownnz_src[] = {2, 2, 2}; const int rowadr_src[] = {0, 2, 4}; const int colind_src[] = {0, 1, 1, 2, 0, 2}; // res = [[1, 0, 0], // [0, 3, 0], // [5, 0, 6]] constexpr int nnz_res = 4; const int rownnz_res[] = {1, 1, 2}; const int rowadr_res[] = {0, 1, 2}; const int colind_res[] = {0, 1, 0, 2}; int map[nnz_res]; mju_sparseMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src, rownnz_src, colind_src); // Expected map: // res[0] (1 at 0,0) -> src[0] (1 at 0,0) => map[0] = 0 // res[1] (3 at 1,1) -> src[2] (3 at 1,1) => map[1] = 2 // res[2] (5 at 2,0) -> src[4] (5 at 2,0) => map[2] = 4 // res[3] (6 at 2,2) -> src[5] (6 at 2,2) => map[3] = 5 EXPECT_THAT(map, ElementsAre(0, 2, 4, 5)); // Verify the map by checking values mjtNum mat_res_gathered[nnz_res]; mju_gather(mat_res_gathered, mat_src, map, nnz_res); EXPECT_THAT(AsVector(mat_res_gathered, nnz_res), ElementsAre(1, 3, 5, 6)); } TEST_F(UtilMiscTest, MjuSparseLower2SymMap) { // nr = 3 // src = [[1, 0, 0], // [2, 3, 0], // [4, 5, 6]] constexpr int nr = 3; const mjtNum mat_src[] = {1, 2, 3, 4, 5, 6}; const int rownnz_src[] = {1, 2, 3}; const int rowadr_src[] = {0, 1, 3}; const int colind_src[] = {0, 0, 1, 0, 1, 2}; // res = [[*, *, *], // [*, *, *], // [*, *, *]] (dense symmetric) constexpr int res_nnz = 9; const int rownnz_res[] = {3, 3, 3}; const int rowadr_res[] = {0, 3, 6}; const int colind_res[] = {0, 1, 2, 0, 1, 2, 0, 1, 2}; int map[res_nnz]; int cursor[nr]; mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src, rownnz_src, colind_src, cursor); // Expected map: // res(0,0) -> src(0,0) (k=0) => map[0] = 0 // res(0,1) -> src(1,0) (k=1) => map[1] = 1 // res(0,2) -> src(2,0) (k=3) => map[2] = 3 // res(1,0) -> src(1,0) (k=1) => map[3] = 1 // res(1,1) -> src(1,1) (k=2) => map[4] = 2 // res(1,2) -> src(2,1) (k=4) => map[5] = 4 // res(2,0) -> src(2,0) (k=3) => map[6] = 3 // res(2,1) -> src(2,1) (k=4) => map[7] = 4 // res(2,2) -> src(2,2) (k=5) => map[8] = 5 EXPECT_THAT(map, ElementsAre(0, 1, 3, 1, 2, 4, 3, 4, 5)); // Verify the map by checking values mjtNum mat_res[res_nnz]; mju_gatherMasked(mat_res, mat_src, map, res_nnz); EXPECT_THAT(AsVector(mat_res, res_nnz), ElementsAre(1, 2, 4, 2, 3, 5, 4, 5, 6)); } TEST_F(UtilMiscTest, MjuSparseLower2SymMapPartial) { // nr = 3 // src = [[1, 0, 0], // [2, 3, 0], // [0, 0, 6]] constexpr int nr = 3; const mjtNum mat_src[] = {1, 2, 3, 6}; const int rownnz_src[] = {1, 2, 1}; const int rowadr_src[] = {0, 1, 3}; const int colind_src[] = {0, 0, 1, 2}; // res with a sparse symmetric pattern // res = [[*, *, *], // [*, *, 0], // [*, 0, *]] constexpr int res_nnz = 7; const int rownnz_res[] = {3, 2, 2}; const int rowadr_res[] = {0, 3, 5}; const int colind_res[] = {0, 1, 2, 0, 1, 0, 2}; int map[res_nnz]; int cursor[nr]; mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src, rownnz_src, colind_src, cursor); // Expected map for the non-zeros in res: // res(0,0) -> src(0,0) (k=0) => map[0] = 0 // res(0,1) -> src(1,0) (k=1) => map[1] = 1 // res(0,2) -> Unmapped => map[2] = -1 // res(1,0) -> src(1,0) (k=1) => map[3] = 1 // res(1,1) -> src(1,1) (k=2) => map[4] = 2 // res(2,0) -> Unmapped => map[5] = -1 // res(2,2) -> src(2,2) (k=3) => map[6] = 3 EXPECT_THAT(map, ElementsAre(0, 1, -1, 1, 2, -1, 3)); // Verify the map by checking values mjtNum mat_res[res_nnz]; mju_gatherMasked(mat_res, mat_src, map, res_nnz); // Expected res values based on map: // mat_res[0] = mat_src[0] = 1 // mat_res[1] = mat_src[1] = 2 // mat_res[2] = 0 (unmapped) // mat_res[3] = mat_src[1] = 2 // mat_res[4] = mat_src[2] = 3 // mat_res[5] = 0 (unmapped) // mat_res[6] = mat_src[3] = 6 EXPECT_THAT(AsVector(mat_res, res_nnz), ElementsAre(1, 2, 0, 2, 3, 0, 6)); } TEST_F(UtilMiscTest, MjuIsZero) { mjtNum vec[1] = {1}; EXPECT_EQ(mju_isZero(vec, 1), 0); EXPECT_EQ(mju_isZero(vec, 0), 1); vec[0] = 0; EXPECT_EQ(mju_isZero(vec, 1), 1); vec[0] = -0.0; EXPECT_EQ(mju_isZero(vec, 1), 1); EXPECT_EQ(mju_isZeroByte((const unsigned char*)vec, sizeof(mjtNum)), 0); } TEST_F(UtilMiscTest, MjuIsZeroByte) { // Zero length array EXPECT_TRUE(mju_isZeroByte(nullptr, 0)); // zero length array with non-null pointer unsigned char vec0[1] = {0}; EXPECT_TRUE(mju_isZeroByte(vec0, sizeof(vec0))); // one zero element array unsigned char vec1[1] = {0}; EXPECT_TRUE(mju_isZeroByte(vec1, sizeof(vec1))); // one non-zero element array unsigned char vec2[2] = {1}; EXPECT_FALSE(mju_isZeroByte(vec2, sizeof(vec2))); // Non-zero at start unsigned char vec3[3] = {1, 0, 0}; EXPECT_FALSE(mju_isZeroByte(vec3, sizeof(vec3))); // Non-zero at end unsigned char vec4[3] = {0, 0, 1}; EXPECT_FALSE(mju_isZeroByte(vec4, sizeof(vec4))); // Non-zero in middle unsigned char vec5[3] = {0, 1, 0}; EXPECT_FALSE(mju_isZeroByte(vec5, sizeof(vec5))); } // --------------------------------- Interpolation ----------------------------- using InterpolationTest = MujocoTest; TEST_F(InterpolationTest, mju_interpolate3D) { // quadratic functions should be interpolated exactly if order = 2 auto quadratic_function_1 = [](mjtNum x, mjtNum y, mjtNum z) { return x*x + y*y + z*z; }; auto quadratic_function_2 = [](mjtNum x, mjtNum y, mjtNum z) { return x*y*z + y*z*z + x*z*z; }; auto quadratic_function_3 = [](mjtNum x, mjtNum y, mjtNum z) { return x*y*z + y*z*z + x*z*z + y*y*z + x*x*z + x + y + z; }; static constexpr int order = 2; mjtNum coeff[3*(order+1)*(order+1)*(order+1)]; int index = 0; for (int i = 0; i <= order; ++i) { for (int j = 0; j <= order; ++j) { for (int k = 0; k <= order; ++k) { coeff[3*index+0] = quadratic_function_1(.5*i, .5*j, .5*k); coeff[3*index+1] = quadratic_function_2(.5*i, .5*j, .5*k); coeff[3*index+2] = quadratic_function_3(.5*i, .5*j, .5*k); index++; } } } static constexpr int nsample = 5; for (int i = 0; i < nsample; ++i) { mjtNum sample[3]; mjtNum expected[3]; mjtNum res[3] = {0}; sample[0] = mju_Halton(i, 2); sample[1] = mju_Halton(i, 3); sample[2] = mju_Halton(i, 5); expected[0] = quadratic_function_1(sample[0], sample[1], sample[2]); expected[1] = quadratic_function_2(sample[0], sample[1], sample[2]); expected[2] = quadratic_function_3(sample[0], sample[1], sample[2]); mju_interpolate3D(res, sample, coeff, order); EXPECT_NEAR(res[0], expected[0], 1e-10); EXPECT_NEAR(res[1], expected[1], 1e-10); EXPECT_NEAR(res[2], expected[2], 1e-10); } } TEST_F(InterpolationTest, mju_defGradient) { int order = 1; mjtNum mat[9]; mjtNum p1[3] = {.5, .5, .5}; mjtNum p2[3] = {.25, .25, .25}; mjtNum dof0[24] = {0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 1, 1, 1, 0, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1}; // identity mjtNum dof1[24]; for (int i = 0; i < 24; ++i) dof1[i] = dof0[i]; mju_defGradient(mat, p1, dof1, order); EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1})); // translation mjtNum dof2[24]; for (int i = 0; i < 24; ++i) dof2[i] = 2 + dof0[i]; mju_defGradient(mat, p1, dof2, order); EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1})); mju_defGradient(mat, p2, dof2, order); EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1})); // constant stretch mjtNum dof3[24]; for (int i = 0; i < 24; ++i) dof3[i] = 2*dof0[i]; mju_defGradient(mat, p1, dof3, order); EXPECT_THAT(mat, ElementsAreArray({2, 0, 0, 0, 2, 0, 0, 0, 2})); mju_defGradient(mat, p2, dof3, order); EXPECT_THAT(mat, ElementsAreArray({2, 0, 0, 0, 2, 0, 0, 0, 2})); // axial stretch mjtNum dof4[24]; for (int i = 0; i < 24; ++i) dof4[i] = (i%3 == 1 ? 2 : 1)*dof0[i]; mju_defGradient(mat, p1, dof4, order); EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 2, 0, 0, 0, 1})); mju_defGradient(mat, p2, dof4, order); EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 2, 0, 0, 0, 1})); // z-axis 90 degree rotation mjtNum dof5[24]; for (int i = 0; i < 8; ++i) { mjtNum quat[4] = {0, 0, 0, 1}; mjtNum axis[3] = {0, 0, 1}; mju_axisAngle2Quat(quat, axis, mjPI/2); mju_rotVecQuat(dof5 + 3*i, dof0 + 3*i, quat); } mju_defGradient(mat, p1, dof5, order); EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), {0, -1, 0, 1, 0, 0, 0, 0, 1})); mju_defGradient(mat, p2, dof5, order); EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), {0, -1, 0, 1, 0, 0, 0, 0, 1})); // z-axis 30 degree rotation mjtNum dof6[24]; mjtNum rot6[9]; for (int i = 0; i < 8; ++i) { mjtNum quat[4]; mjtNum axis[3] = {0, 0, 1}; mju_axisAngle2Quat(quat, axis, mjPI/6); mju_rotVecQuat(dof6 + 3*i, dof0 + 3*i, quat); mju_quat2Mat(rot6, quat); } mju_defGradient(mat, p1, dof6, order); EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot6)); mju_defGradient(mat, p2, dof6, order); EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot6)); // z-axis CoM rotation mjtNum dof7[24]; mjtNum rot7[9]; for (int i = 0; i < 8; ++i) { mjtNum quat[4]; mjtNum axis[3] = {0, 0, 1}; mjtNum offset[3] = {-.5, -.5, 0}; mju_axisAngle2Quat(quat, axis, mjPI/6); mju_add3(dof7 + 3*i, dof0 + 3*i, offset); mju_rotVecQuat(dof7 + 3*i, dof0 + 3*i, quat); mju_quat2Mat(rot7, quat); } mju_defGradient(mat, p1, dof7, order); EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot7)); mju_defGradient(mat, p2, dof7, order); EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot7)); } // --------------------------------- Base64 ------------------------------------ using Base64Test = MujocoTest; TEST_F(Base64Test, mju_encodeBase64) { std::array buffer; std::array arr = {15, 134, 190, 255, 240}; std::size_t n = mju_encodeBase64(buffer.data(), arr.data(), arr.size()); EXPECT_THAT(buffer.data(), StrEq("D4a+//A=")); EXPECT_THAT(n, std::strlen(buffer.data()) + 1); EXPECT_THAT(n, buffer.size()); } TEST_F(Base64Test, mju_encodeBase64_align0) { std::array buffer; std::array arr = {'A', 'B', 'C'}; std::size_t n = mju_encodeBase64(buffer.data(), arr.data(), arr.size()); EXPECT_THAT(buffer.data(), StrEq("QUJD")); EXPECT_THAT(n, std::strlen(buffer.data()) + 1); EXPECT_THAT(n, buffer.size()); } TEST_F(Base64Test, mju_encodeBase64_align1) { std::array buffer; std::array arr = {'A', 'B'}; std::size_t n = mju_encodeBase64(buffer.data(), arr.data(), arr.size()); EXPECT_THAT(buffer.data(), StrEq("QUI=")); EXPECT_THAT(n, std::strlen(buffer.data()) + 1); EXPECT_THAT(n, buffer.size()); } TEST_F(Base64Test, mju_encodeBase64_align2) { std::array buffer; std::array arr = {'A'}; std::size_t n = mju_encodeBase64(buffer.data(), arr.data(), arr.size()); EXPECT_THAT(buffer.data(), StrEq("QQ==")); EXPECT_THAT(n, std::strlen(buffer.data()) + 1); EXPECT_THAT(n, buffer.size()); } TEST_F(Base64Test, mju_encodeBase64_null) { std::array buffer; std::size_t n = mju_encodeBase64(buffer.data(), NULL, 0); EXPECT_THAT(n, 1); EXPECT_THAT(buffer[0], '\0'); } TEST_F(Base64Test, mju_encodeBase64_ones) { std::array buffer; std::array arr = {255, 255, 255}; std::size_t n = mju_encodeBase64(buffer.data(), arr.data(), arr.size()); EXPECT_THAT(buffer.data(), StrEq("////")); EXPECT_THAT(n, std::strlen(buffer.data()) + 1); EXPECT_THAT(n, buffer.size()); } TEST_F(Base64Test, mju_isValidBase64_emptyStr) { std::size_t n = mju_isValidBase64(""); EXPECT_THAT(n, 0); } TEST_F(Base64Test, mju_isValidBase64_invalid1) { std::size_t n = mju_isValidBase64("A"); EXPECT_THAT(n, 0); } TEST_F(Base64Test, mju_isValidBase64_invalid2) { std::size_t n = mju_isValidBase64("AAA"); EXPECT_THAT(n, 0); } TEST_F(Base64Test, mju_isValidBase64_invalid3) { std::size_t n = mju_isValidBase64("A==A"); EXPECT_THAT(n, 0); } TEST_F(Base64Test, mju_isValidBase64_invalid5) { std::size_t n = mju_isValidBase64("A==="); EXPECT_THAT(n, 0); } TEST_F(Base64Test, mju_isValidBase64_invalid6) { std::size_t n = mju_isValidBase64("aaaa===="); EXPECT_THAT(n, 0); } TEST_F(Base64Test, mju_isValidBase64_invalid7) { std::size_t n = mju_isValidBase64("A#AA"); EXPECT_THAT(n, 0); } TEST_F(Base64Test, mju_isValidBase64_valid1) { std::size_t n = mju_isValidBase64("AB+/"); EXPECT_THAT(n, 3); } TEST_F(Base64Test, mju_isValidBase64_valid2) { std::size_t n = mju_isValidBase64("ABC="); EXPECT_THAT(n, 2); } TEST_F(Base64Test, mju_isValidBase64_valid3) { std::size_t n = mju_isValidBase64("AB=="); EXPECT_THAT(n, 1); } TEST_F(Base64Test, mju_isValidBase64_valid4) { std::size_t n = mju_isValidBase64("az09AZ+/11=="); EXPECT_THAT(n, 7); } TEST_F(Base64Test, mju_decodeBase64) { std::array buffer; const char *s = "D4a+//A="; std::size_t n = mju_decodeBase64(buffer.data(), s); EXPECT_THAT(buffer, ElementsAreArray({15, 134, 190, 255, 240})); EXPECT_THAT(n, buffer.size()); } TEST_F(Base64Test, mju_decodeBase6_align0) { std::array buffer; const char *s = "QUJD"; std::size_t n = mju_decodeBase64(buffer.data(), s); EXPECT_THAT(buffer, ElementsAreArray({'A', 'B', 'C'})); EXPECT_THAT(n, buffer.size()); } TEST_F(Base64Test, mju_decodeBase64_align1) { std::array buffer; const char *s = "QUI="; std::size_t n = mju_decodeBase64(buffer.data(), s); EXPECT_THAT(buffer, ElementsAreArray({'A', 'B'})); EXPECT_THAT(n, buffer.size()); } TEST_F(Base64Test, mju_decodeBase64_align2) { std::array buffer; const char *s = "QQ=="; std::size_t n = mju_decodeBase64(buffer.data(), s); EXPECT_THAT(buffer, ElementsAreArray({'A'})); EXPECT_THAT(n, buffer.size()); } TEST_F(Base64Test, mju_decodeBase64_null) { const char *s = ""; std::size_t n = mju_decodeBase64(NULL, s); EXPECT_THAT(n, 0); } TEST_F(Base64Test, mju_decodeBase64_ones) { std::array buffer; const char *s = "////"; std::size_t n = mju_decodeBase64(buffer.data(), s); EXPECT_THAT(buffer, ElementsAreArray({255, 255, 255})); EXPECT_THAT(n, buffer.size()); } TEST_F(Base64Test, decodeAndEncode) { std::array buffer1; std::array buffer2; const char *s = "D4a+/vA="; mju_decodeBase64(buffer1.data(), s); mju_encodeBase64(buffer2.data(), buffer1.data(), buffer1.size()); EXPECT_THAT(buffer2.data(), StrEq(s)); } // --------------------------------- History Buffers --------------------------- using HistoryTest = MujocoTest; // buffer layout: [user(1), cursor(1), times(n), values(n*dim)] // cursor points to newest element (logical index n-1) // after init, cursor=n-1, so physical indices equal logical indices TEST_F(HistoryTest, Init) { constexpr int n = 4; constexpr int dim = 1; mjtNum buf[2 + n + n*dim]; std::vector times = {4, 6, 8, 10}; std::vector values = {99, 99, 99, 99}; mju_historyInit(buf, n, dim, times.data(), values.data(), 0.0); // check header EXPECT_EQ(buf[0], 0.0); // user EXPECT_EQ(buf[1], static_cast(n-1)); // cursor = n-1 // timestamps: [4, 6, 8, 10] (t=10 is newest) // values: [99, 99, 99, 99] // verify via read function (logical order) mjtNum res; EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 99.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 99.0); } TEST_F(HistoryTest, Init_Vector) { constexpr int n = 3; constexpr int dim = 2; mjtNum buf[2 + n + n*dim]; std::vector times = {-2, -1, 0}; std::vector values = {1.0, 2.0, 1.0, 2.0, 1.0, 2.0}; mju_historyInit(buf, n, dim, times.data(), values.data(), 0.0); EXPECT_EQ(buf[1], static_cast(n-1)); // cursor = n-1 // verify via read function mjtNum res[dim]; const mjtNum* ptr = mju_historyRead(buf, n, dim, res, -2.0, 0); ASSERT_NE(ptr, nullptr); EXPECT_EQ(ptr[0], 1.0); EXPECT_EQ(ptr[1], 2.0); } TEST_F(HistoryTest, Append) { constexpr int n = 4; constexpr int dim = 1; // Initialize buffer properly, then insert mjtNum buf[2 + 2*n]; buf[0] = 0.0; buf[1] = n - 1; // timestamps: [4, 6, 8, 10] mjtNum times[] = {4, 6, 8, 10}; mju_copy(buf + 2, times, n); // values: [0, 0, 0, 0] mju_zero(buf + 2 + n, n); // overwrite with specific values *mju_historyInsert(buf, n, dim, 4.0) = 1.0; *mju_historyInsert(buf, n, dim, 6.0) = 2.0; *mju_historyInsert(buf, n, dim, 8.0) = 3.0; *mju_historyInsert(buf, n, dim, 10.0) = 4.0; // now append at t=12 *mju_historyInsert(buf, n, dim, 12.0) = 99.0; // verify logical order: [6, 8, 10, 12] -> [2, 3, 4, 99] mjtNum res; EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 12.0, 0), 99.0); // oldest should now be t=6 EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 2.0); } TEST_F(HistoryTest, Append_Multiple) { constexpr int n = 3; constexpr int dim = 1; mjtNum buf[2 + 2*n]; buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {-2, -1, 0}; mju_copy(buf + 2, times, n); mju_zero(buf + 2 + n, n); for (int i = 1; i <= 4; i++) { mjtNum i_real = static_cast(i); *mju_historyInsert(buf, n, dim, i_real) = i_real; } // Final: logical timestamps [2, 3, 4], values [2, 3, 4] mjtNum res; EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 2.0, 0), 2.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 3.0, 0), 3.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 4.0); } TEST_F(HistoryTest, ReadVector_ExactMatch) { constexpr int n = 3; constexpr int dim = 2; mjtNum buf[2 + n + n*dim]; buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {0, 1, 2}; mju_copy(buf + 2, times, n); mju_zero(buf + 2 + n, n*dim); // set values: t=0->(1,2), t=1->(3,4), t=2->(5,6) mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); slot0[0] = 1.0; slot0[1] = 2.0; mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); slot1[0] = 3.0; slot1[1] = 4.0; mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0); slot2[0] = 5.0; slot2[1] = 6.0; mjtNum res[dim]; const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 1.0, 0); ASSERT_NE(ptr, nullptr); EXPECT_EQ(ptr[0], 3.0); EXPECT_EQ(ptr[1], 4.0); } TEST_F(HistoryTest, ReadVector_ZOH) { constexpr int n = 3; constexpr int dim = 2; mjtNum buf[2 + n + n*dim]; buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {0, 1, 2}; mju_copy(buf + 2, times, n); mju_zero(buf + 2 + n, n*dim); mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); slot0[0] = 1.0; slot0[1] = 2.0; mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); slot1[0] = 3.0; slot1[1] = 4.0; mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0); slot2[0] = 5.0; slot2[1] = 6.0; mjtNum res[dim]; const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 0); ASSERT_NE(ptr, nullptr); EXPECT_EQ(ptr[0], 1.0); EXPECT_EQ(ptr[1], 2.0); } TEST_F(HistoryTest, ReadVector_Linear) { constexpr int n = 3; constexpr int dim = 2; mjtNum buf[2 + n + n*dim]; buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {0, 1, 2}; mju_copy(buf + 2, times, n); mju_zero(buf + 2 + n, n*dim); mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); slot0[0] = 1.0; slot0[1] = 2.0; mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); slot1[0] = 3.0; slot1[1] = 4.0; mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0); slot2[0] = 5.0; slot2[1] = 6.0; mjtNum res[dim]; const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 1); EXPECT_EQ(ptr, nullptr); EXPECT_THAT(res[0], DoubleNear(2.0, 1e-10)); // (1+3)/2 EXPECT_THAT(res[1], DoubleNear(3.0, 1e-10)); // (2+4)/2 } TEST_F(HistoryTest, InsertOutOfOrder) { constexpr int n = 4; constexpr int dim = 1; mjtNum buf[2 + 2*n]; mjtNum res; auto reset = [&]() { buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {4, 6, 8, 10}; mju_copy(buf + 2, times, n); mju_zero(buf + 2 + n, n); *mju_historyInsert(buf, n, dim, 4.0) = 1.0; *mju_historyInsert(buf, n, dim, 6.0) = 2.0; *mju_historyInsert(buf, n, dim, 8.0) = 3.0; *mju_historyInsert(buf, n, dim, 10.0) = 4.0; }; // insert in middle (between t=8 and t=10) reset(); *mju_historyInsert(buf, n, dim, 9.0) = 99.0; // logical: [6, 8, 9, 10] -> [2, 3, 99, 4] EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 9.0, 0), 99.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0); // insert near start (between t=4 and t=6) reset(); *mju_historyInsert(buf, n, dim, 5.0) = 99.0; // logical: [5, 6, 8, 10] -> [99, 2, 3, 4] EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 5.0, 0), 99.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0); // insert before oldest (t=3 < t=4): replaces oldest reset(); *mju_historyInsert(buf, n, dim, 3.0) = 99.0; // logical: [3, 6, 8, 10] -> [99, 2, 3, 4] EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 3.0, 0), 99.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0); } TEST_F(HistoryTest, InsertReplaceOnCollision) { constexpr int n = 4; constexpr int dim = 1; mjtNum buf[2 + 2*n]; mjtNum res; auto reset = [&]() { // timestamps: [4, 6, 8, 10], values initialized to 0 buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {4, 6, 8, 10}; mju_copy(buf + 2, times, n); mju_zero(buf + 2 + n, n); *mju_historyInsert(buf, n, dim, 4.0) = 1.0; *mju_historyInsert(buf, n, dim, 6.0) = 2.0; *mju_historyInsert(buf, n, dim, 8.0) = 3.0; *mju_historyInsert(buf, n, dim, 10.0) = 4.0; }; // collision in middle (t=8) reset(); *mju_historyInsert(buf, n, dim, 8.0) = 99.0; EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 1.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 99.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0); // collision at newest (t=10) reset(); *mju_historyInsert(buf, n, dim, 10.0) = 99.0; EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 1.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 99.0); // collision at oldest (t=4) reset(); *mju_historyInsert(buf, n, dim, 4.0) = 99.0; EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 4.0, 0), 99.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 6.0, 0), 2.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 8.0, 0), 3.0); EXPECT_EQ(*mju_historyRead(buf, n, dim, &res, 10.0, 0), 4.0); } void TriggerHistoryInitNonMonotonic() { mjtNum buf[10]; mjtNum times[4] = {1, 2, 2, 4}; // not strictly increasing mjtNum values[4] = {0}; mju_historyInit(buf, 4, 1, times, values, 0.0); } TEST_F(HistoryTest, Init_NonMonotonic) { EXPECT_FATAL_FAILURE(TriggerHistoryInitNonMonotonic(), "mju_historyInit: times must be strictly increasing"); } TEST_F(HistoryTest, CubicInterpolation) { int n = 2; int dim = 2; mjtNum buf[100]; // 2 + 2 + 2*2 = 8 buf[0] = 0.0; buf[1] = n - 1; mjtNum times[] = {-1, 0}; mju_copy(buf + 2, times, n); mju_zero(buf + 2 + n, n*dim); // Insert (0, 0, 1) and (1, 1, 0). // Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3 // Dim 1: 1 -> 0. Spline: p(x) = 1 - 3x^2 + 2x^3 mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); slot0[0] = 0.0; slot0[1] = 1.0; mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0); slot1[0] = 1.0; slot1[1] = 0.0; mjtNum res[2]; // Test midpoint x=0.5 // Dim 0: 0.5 // Dim 1: 1 - 0.5 = 0.5 mju_historyRead(buf, n, dim, res, 0.5, 2); EXPECT_NEAR(res[0], 0.5, 1e-9); EXPECT_NEAR(res[1], 0.5, 1e-9); // Test x=0.25 // Dim 0: 3*0.25^2 - 2*0.25^3 // Dim 1: 1 - (3*0.25^2 - 2*0.25^3) mju_historyRead(buf, n, dim, res, 0.25, 2); mjtNum expected_0_25 = 3*0.25*0.25 - 2*0.25*0.25*0.25; EXPECT_NEAR(res[0], expected_0_25, 1e-9); EXPECT_NEAR(res[1], 1.0 - expected_0_25, 1e-9); // Test x=0.8 // Dim 0: 3*0.8^2 - 2*0.8^3 // Dim 1: 1 - (3*0.8^2 - 2*0.8^3) mju_historyRead(buf, n, dim, res, 0.8, 2); mjtNum expected_0_8 = 3*0.8*0.8 - 2*0.8*0.8*0.8; EXPECT_NEAR(res[0], expected_0_8, 1e-9); EXPECT_NEAR(res[1], 1.0 - expected_0_8, 1e-9); } } // namespace } // namespace mujoco