Yuval Tassa
1ea2d884d2
PiperOrigin-RevId: 942873144 Change-Id: I3fd0d630643c104ff464d619387ff36582c2a5d8
1749 lines
54 KiB
C++
1749 lines
54 KiB
C++
// 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 "src/engine/engine_util_misc.h"
|
|
|
|
#include <array>
|
|
#include <climits>
|
|
#include <cmath>
|
|
#include <cstddef>
|
|
#include <cstdint>
|
|
#include <cstring>
|
|
#include <vector>
|
|
|
|
#include <gmock/gmock.h>
|
|
#include <gtest/gtest-spi.h>
|
|
#include <gtest/gtest.h>
|
|
#include <mujoco/mjdata.h>
|
|
#include <mujoco/mujoco.h>
|
|
#include "test/fixture.h"
|
|
|
|
namespace mujoco {
|
|
namespace {
|
|
|
|
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 = MjTol(1e-7, 1e-3);
|
|
const mjtNum fd_tol = MjTol(1e-7, 1e-3);
|
|
|
|
// derivative at 0
|
|
mjtNum dy_dx_0 = (mju_sigmoid(0 + dx) - mju_sigmoid(0)) / dx;
|
|
EXPECT_NEAR(dy_dx_0, 0, fd_tol);
|
|
|
|
// derivative at 1
|
|
mjtNum dy_dx_1 = (mju_sigmoid(1) - mju_sigmoid(1 - dx)) / dx;
|
|
EXPECT_NEAR(dy_dx_1, 0, fd_tol);
|
|
|
|
// 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_NEAR(dy_dx_0p5, expected, fd_tol);
|
|
}
|
|
|
|
TEST_F(UtilMiscTest, SphereWrap) {
|
|
static constexpr char xml[] = R"(
|
|
<mujoco>
|
|
<default>
|
|
<site size=".015" rgba="1 0 0 1"/>
|
|
</default>
|
|
|
|
<worldbody>
|
|
<light pos="0 0 3"/>
|
|
|
|
<site name="fixed" pos="0 0 1"/>
|
|
<geom name="sphere" size=".1" pos="0 0 0.5"/>
|
|
<site name="sidesite" pos=".52 0 .5"/>
|
|
<body pos="0 0 .1">
|
|
<freejoint/>
|
|
<geom size=".05"/>
|
|
<site name="body" pos="0 0 .05"/>
|
|
</body>
|
|
</worldbody>
|
|
|
|
<tendon>
|
|
<spatial name="tendon" range="0 0.8">
|
|
<site site="fixed"/>
|
|
<geom geom="sphere" sidesite="sidesite"/>
|
|
<site site="body"/>
|
|
</spatial>
|
|
</tendon>
|
|
|
|
<sensor>
|
|
<tendonpos tendon="tendon"/>
|
|
</sensor>
|
|
|
|
<keyframe>
|
|
<key qpos="-0.00653537 -0.068031 0.301253 0.982186 -0.180204 -0.0273515 0.0457068"/>
|
|
<key qpos="-0.00653537 -0.069 0.301253 0.982186 -0.180204 -0.0273515 0.0457068"/>
|
|
</keyframe>
|
|
</mujoco>
|
|
)";
|
|
|
|
char error[1024];
|
|
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
|
|
ASSERT_THAT(model.get(), NotNull()) << error;
|
|
MjDataPtr data = MakeData(model);
|
|
|
|
// measure tendon length for keyframe 0
|
|
mj_resetDataKeyframe(model.get(), data.get(), 0);
|
|
mj_forward(model.get(), data.get());
|
|
mjtNum ten_length0 = data->sensordata[0];
|
|
|
|
// measure tendon length for keyframe 1
|
|
mj_resetDataKeyframe(model.get(), data.get(), 1);
|
|
mj_forward(model.get(), data.get());
|
|
mjtNum ten_length1 = data->sensordata[0];
|
|
|
|
// difference should be small
|
|
mjtNum diff = ten_length1 - ten_length0;
|
|
EXPECT_LT(mju_abs(diff), 1e-3);
|
|
}
|
|
|
|
// 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, NULL);
|
|
EXPECT_NEAR(res[0], expected[0], MjTol(1e-10, 1e-5));
|
|
EXPECT_NEAR(res[1], expected[1], MjTol(1e-10, 1e-5));
|
|
EXPECT_NEAR(res[2], expected[2], MjTol(1e-10, 1e-5));
|
|
}
|
|
}
|
|
|
|
TEST_F(InterpolationTest, mju_cellLookup_SingleCell) {
|
|
// single cell (1x1x1): local coords should equal global coords
|
|
int cellnum[3] = {1, 1, 1};
|
|
mjtNum coord[3] = {0.3, 0.7, 0.5};
|
|
mjtNum local[3];
|
|
int nodeindices[8];
|
|
|
|
int npc = mju_cellLookup(coord, cellnum, 1, local, nodeindices);
|
|
EXPECT_EQ(npc, 8);
|
|
EXPECT_NEAR(local[0], 0.3, MjTol(1e-12, 1e-6));
|
|
EXPECT_NEAR(local[1], 0.7, MjTol(1e-12, 1e-6));
|
|
EXPECT_NEAR(local[2], 0.5, MjTol(1e-12, 1e-6));
|
|
|
|
// for trilinear 1x1x1: nodes are 0..7 in lexicographic order
|
|
for (int i = 0; i < 8; i++) {
|
|
EXPECT_EQ(nodeindices[i], i);
|
|
}
|
|
}
|
|
|
|
TEST_F(InterpolationTest, mju_cellLookup_MultiCell) {
|
|
// 2x3x4 grid, trilinear: 3x4x5 = 60 nodes
|
|
int cellnum[3] = {2, 3, 4};
|
|
int order = 1;
|
|
int ny_g = 3 * 1 + 1; // 4
|
|
int nz_g = 4 * 1 + 1; // 5
|
|
|
|
// point at (0.75, 0.5, 0.125) -> cell (1, 1, 0)
|
|
mjtNum coord[3] = {0.75, 0.5, 0.125};
|
|
mjtNum local[3];
|
|
int nodeindices[8];
|
|
|
|
int npc = mju_cellLookup(coord, cellnum, order, local, nodeindices);
|
|
EXPECT_EQ(npc, 8);
|
|
|
|
// cell (1,1,0): local = (0.75*2 - 1, 0.5*3 - 1, 0.125*4 - 0)
|
|
EXPECT_NEAR(local[0], 0.5, 1e-12);
|
|
EXPECT_NEAR(local[1], 0.5, 1e-12);
|
|
EXPECT_NEAR(local[2], 0.5, 1e-12);
|
|
|
|
// expected node indices for cell (1,1,0), trilinear:
|
|
// (gi, gj, gk) for li,lj,lk in {0,1}
|
|
// gi = 1+li, gj = 1+lj, gk = 0+lk
|
|
// gidx = gi*ny_g*nz_g + gj*nz_g + gk
|
|
int expected[8];
|
|
int ni = 0;
|
|
for (int li = 0; li <= 1; li++) {
|
|
for (int lj = 0; lj <= 1; lj++) {
|
|
for (int lk = 0; lk <= 1; lk++) {
|
|
expected[ni++] = (1 + li) * ny_g * nz_g + (1 + lj) * nz_g + lk;
|
|
}
|
|
}
|
|
}
|
|
for (int i = 0; i < 8; i++) {
|
|
EXPECT_EQ(nodeindices[i], expected[i]);
|
|
}
|
|
}
|
|
|
|
TEST_F(InterpolationTest, mju_cellLookup_Boundary) {
|
|
// point exactly at coord=1.0 should clamp to last cell
|
|
int cellnum[3] = {3, 3, 3};
|
|
mjtNum coord[3] = {1.0, 1.0, 1.0};
|
|
mjtNum local[3];
|
|
|
|
mju_cellLookup(coord, cellnum, 1, local, NULL);
|
|
// cell (2,2,2), local = (1*3 - 2, 1*3 - 2, 1*3 - 2) = (1, 1, 1)
|
|
EXPECT_NEAR(local[0], 1.0, 1e-12);
|
|
EXPECT_NEAR(local[1], 1.0, 1e-12);
|
|
EXPECT_NEAR(local[2], 1.0, 1e-12);
|
|
|
|
// point at coord=0.0 should map to first cell
|
|
mjtNum coord0[3] = {0.0, 0.0, 0.0};
|
|
mju_cellLookup(coord0, cellnum, 1, local, NULL);
|
|
EXPECT_NEAR(local[0], 0.0, 1e-12);
|
|
EXPECT_NEAR(local[1], 0.0, 1e-12);
|
|
EXPECT_NEAR(local[2], 0.0, 1e-12);
|
|
}
|
|
|
|
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(MjNear(1e-8, 1e-6), {0, -1, 0, 1, 0, 0, 0, 0, 1}));
|
|
mju_defGradient(mat, p2, dof5, order);
|
|
EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), {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(MjNear(1e-8, 1e-6), rot6));
|
|
mju_defGradient(mat, p2, dof6, order);
|
|
EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), 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(MjNear(1e-8, 1e-6), rot7));
|
|
mju_defGradient(mat, p2, dof7, order);
|
|
EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), rot7));
|
|
}
|
|
|
|
TEST_F(InterpolationTest, mju_flexInterpState_MultiCell) {
|
|
int order = 1; // trilinear
|
|
int cy = 2;
|
|
int cz = 2;
|
|
int nodenum = 27; // 3x3x3
|
|
|
|
std::vector<mjtNum> xpos(3 * nodenum);
|
|
mjtNum quat[4];
|
|
|
|
// Populate xpos directly for a grid centered at origin, rotated 90 deg around
|
|
// Z Original grid points: {-0.1, 0.0, 0.1}^3 Rotated: (x, y, z) -> (-y, x, z)
|
|
int idx = 0;
|
|
for (int i = 0; i < 3; i++) {
|
|
for (int j = 0; j < 3; j++) {
|
|
for (int k = 0; k < 3; k++) {
|
|
mjtNum x = (i - 1) * 0.1;
|
|
mjtNum y = (j - 1) * 0.1;
|
|
mjtNum z = (k - 1) * 0.1;
|
|
|
|
// Apply rotation
|
|
xpos[3 * idx + 0] = -y;
|
|
xpos[3 * idx + 1] = x;
|
|
xpos[3 * idx + 2] = z;
|
|
idx++;
|
|
}
|
|
}
|
|
}
|
|
|
|
int npc = (order + 1) * (order + 1) * (order + 1);
|
|
std::vector<mjtNum> xpos_c(3 * npc);
|
|
|
|
mju_flexGatherCellState(order, cy, cz, 0, 0, 0, xpos.data(), NULL, NULL,
|
|
xpos_c.data(), NULL, NULL, NULL, quat);
|
|
|
|
// Expected quaternion for -90 deg around Z (global to local):
|
|
// [sqrt(0.5), 0, 0, -sqrt(0.5)]
|
|
mjtNum expected_val = mju_sqrt(0.5);
|
|
EXPECT_NEAR(quat[0], expected_val, 1e-5);
|
|
EXPECT_NEAR(quat[1], 0.0, 1e-5);
|
|
EXPECT_NEAR(quat[2], 0.0, 1e-5);
|
|
EXPECT_NEAR(quat[3], -expected_val, 1e-5);
|
|
}
|
|
|
|
using Base64Test = MujocoTest;
|
|
|
|
TEST_F(Base64Test, mju_encodeBase64) {
|
|
std::array<char, 9> buffer;
|
|
std::array<std::uint8_t, 5> 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<char, 5> buffer;
|
|
std::array<std::uint8_t, 3> 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<char, 5> buffer;
|
|
std::array<std::uint8_t, 2> 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<char, 5> buffer;
|
|
std::array<std::uint8_t, 1> 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<char, 1> 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<char, 5> buffer;
|
|
std::array<std::uint8_t, 3> 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<std::uint8_t, 5> 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<std::uint8_t, 3> 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<std::uint8_t, 2> 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<std::uint8_t, 1> 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<std::uint8_t, 3> 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<std::uint8_t, 5> buffer1;
|
|
std::array<char, 9> 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<mjtNum> times = {4, 6, 8, 10};
|
|
std::vector<mjtNum> 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<mjtNum>(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<mjtNum> times = {-2, -1, 0};
|
|
std::vector<mjtNum> 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<mjtNum>(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<mjtNum>(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_NEAR(res[0], 2.0, MjTol(1e-10, 1e-10)); // (1+3)/2
|
|
EXPECT_NEAR(res[1], 3.0, MjTol(1e-10, 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, MjTol(1e-9, 1e-9));
|
|
EXPECT_NEAR(res[1], 0.5, MjTol(1e-9, 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, MjTol(1e-9, 1e-9));
|
|
EXPECT_NEAR(res[1], 1.0 - expected_0_25, MjTol(1e-9, 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, MjTol(1e-9, 1e-9));
|
|
EXPECT_NEAR(res[1], 1.0 - expected_0_8, MjTol(1e-9, 1e-9));
|
|
}
|
|
|
|
// -------------------------------- Face State ---------------------------------
|
|
|
|
using FaceStateTest = MujocoTest;
|
|
|
|
// verify mju_flexGatherFaceState returns correct node indices for all 6 faces
|
|
// of a 1x1x1 trilinear grid (2x2x2 = 8 nodes, 4 nodes per face)
|
|
TEST_F(FaceStateTest, NodeIndicesSingleCell) {
|
|
int order = 1;
|
|
int cx = 1, cy = 1, cz = 1;
|
|
int ny_g = cy * order + 1; // 2
|
|
int nz_g = cz * order + 1; // 2
|
|
|
|
// nelem_fe = 2*(1*1 + 1*1 + 1*1) = 6 face elements
|
|
// face 0: x=0, face 1: x=max, face 2: y=0, face 3: y=max,
|
|
// face 4: z=0, face 5: z=max
|
|
|
|
// create dummy positions for 8 nodes
|
|
std::vector<mjtNum> xpos(3 * 8, 0);
|
|
for (int i = 0; i < 8; i++) {
|
|
xpos[3 * i + 0] = (i / 4) * 1.0;
|
|
xpos[3 * i + 1] = ((i / 2) % 2) * 1.0;
|
|
xpos[3 * i + 2] = (i % 2) * 1.0;
|
|
}
|
|
|
|
// helper: compute expected global node index from (gx, gy, gz)
|
|
auto gidx = [&](int gx, int gy, int gz) {
|
|
return gx * ny_g * nz_g + gy * nz_g + gz;
|
|
};
|
|
|
|
// face 0: x=0 (fixed g[0]=0, varying g[1], g[2])
|
|
// normal_axis=0, na0=1, na1=2
|
|
{
|
|
int indices[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, NULL,
|
|
NULL, NULL, indices, NULL);
|
|
EXPECT_EQ(indices[0], gidx(0, 0, 0));
|
|
EXPECT_EQ(indices[1], gidx(0, 0, 1));
|
|
EXPECT_EQ(indices[2], gidx(0, 1, 0));
|
|
EXPECT_EQ(indices[3], gidx(0, 1, 1));
|
|
}
|
|
|
|
// face 1: x=max (fixed g[0]=1, varying g[1], g[2])
|
|
{
|
|
int indices[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL, NULL,
|
|
NULL, NULL, indices, NULL);
|
|
EXPECT_EQ(indices[0], gidx(1, 0, 0));
|
|
EXPECT_EQ(indices[1], gidx(1, 0, 1));
|
|
EXPECT_EQ(indices[2], gidx(1, 1, 0));
|
|
EXPECT_EQ(indices[3], gidx(1, 1, 1));
|
|
}
|
|
|
|
// face 2: y=0 (fixed g[1]=0)
|
|
// normal_axis=1, na0=2(z slow), na1=0(x fast)
|
|
// loop order: l0→z, l1→x
|
|
{
|
|
int indices[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL, NULL,
|
|
NULL, NULL, indices, NULL);
|
|
EXPECT_EQ(indices[0], gidx(0, 0, 0)); // l0=0(z=0), l1=0(x=0)
|
|
EXPECT_EQ(indices[1], gidx(1, 0, 0)); // l0=0(z=0), l1=1(x=1)
|
|
EXPECT_EQ(indices[2], gidx(0, 0, 1)); // l0=1(z=1), l1=0(x=0)
|
|
EXPECT_EQ(indices[3], gidx(1, 0, 1)); // l0=1(z=1), l1=1(x=1)
|
|
}
|
|
|
|
// face 3: y=max (fixed g[1]=1)
|
|
// normal_axis=1, na0=2(z slow), na1=0(x fast)
|
|
{
|
|
int indices[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, NULL,
|
|
NULL, NULL, indices, NULL);
|
|
EXPECT_EQ(indices[0], gidx(0, 1, 0)); // l0=0(z=0), l1=0(x=0)
|
|
EXPECT_EQ(indices[1], gidx(1, 1, 0)); // l0=0(z=0), l1=1(x=1)
|
|
EXPECT_EQ(indices[2], gidx(0, 1, 1)); // l0=1(z=1), l1=0(x=0)
|
|
EXPECT_EQ(indices[3], gidx(1, 1, 1)); // l0=1(z=1), l1=1(x=1)
|
|
}
|
|
|
|
// face 4: z=0 (fixed g[2]=0, varying g[0], g[1])
|
|
// normal_axis=2, na0=0, na1=1
|
|
{
|
|
int indices[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, NULL,
|
|
NULL, NULL, indices, NULL);
|
|
EXPECT_EQ(indices[0], gidx(0, 0, 0));
|
|
EXPECT_EQ(indices[1], gidx(0, 1, 0));
|
|
EXPECT_EQ(indices[2], gidx(1, 0, 0));
|
|
EXPECT_EQ(indices[3], gidx(1, 1, 0));
|
|
}
|
|
|
|
// face 5: z=max (fixed g[2]=1)
|
|
{
|
|
int indices[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, NULL,
|
|
NULL, NULL, indices, NULL);
|
|
EXPECT_EQ(indices[0], gidx(0, 0, 1));
|
|
EXPECT_EQ(indices[1], gidx(0, 1, 1));
|
|
EXPECT_EQ(indices[2], gidx(1, 0, 1));
|
|
EXPECT_EQ(indices[3], gidx(1, 1, 1));
|
|
}
|
|
}
|
|
|
|
// verify node indices for a multi-cell grid (2x2x2 cells → 3x3x3 = 27 nodes)
|
|
TEST_F(FaceStateTest, NodeIndicesMultiCell) {
|
|
int order = 1;
|
|
int cx = 2, cy = 2, cz = 2;
|
|
int ny_g = 3, nz_g = 3; // (2*1+1) = 3
|
|
|
|
// nelem_fe = 2*(2*2 + 2*2 + 2*2) = 24 face elements
|
|
// face 0: x=0, cy*cz = 4 quads (indices 0-3)
|
|
// face 1: x=max, 4 quads (indices 4-7)
|
|
// face 2: y=0, cx*cz = 4 quads (indices 8-11)
|
|
// face 3: y=max, 4 quads (indices 12-15)
|
|
// face 4: z=0, cx*cy = 4 quads (indices 16-19)
|
|
// face 5: z=max, 4 quads (indices 20-23)
|
|
|
|
std::vector<mjtNum> xpos(3 * 27, 0);
|
|
for (int i = 0; i < 27; i++) {
|
|
int gi = i / 9;
|
|
int gj = (i / 3) % 3;
|
|
int gk = i % 3;
|
|
xpos[3 * i + 0] = gi * 0.1;
|
|
xpos[3 * i + 1] = gj * 0.1;
|
|
xpos[3 * i + 2] = gk * 0.1;
|
|
}
|
|
|
|
auto gidx = [&](int gx, int gy, int gz) {
|
|
return gx * ny_g * nz_g + gy * nz_g + gz;
|
|
};
|
|
|
|
// face 0 (x=0), quad 0: (q0=0, q1=0) within cy*cz face
|
|
// c1 = face_count1[0] = cz = 2, so quad (0,0) → within_face = 0
|
|
// na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1
|
|
{
|
|
int indices[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, NULL,
|
|
NULL, NULL, indices, NULL);
|
|
EXPECT_EQ(indices[0], gidx(0, 0, 0));
|
|
EXPECT_EQ(indices[1], gidx(0, 0, 1));
|
|
EXPECT_EQ(indices[2], gidx(0, 1, 0));
|
|
EXPECT_EQ(indices[3], gidx(0, 1, 1));
|
|
}
|
|
|
|
// face 0 (x=0), quad 3: (q0=1, q1=1) → within_face = 1*2+1 = 3
|
|
{
|
|
int indices[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, NULL,
|
|
NULL, NULL, indices, NULL);
|
|
EXPECT_EQ(indices[0], gidx(0, 1, 1));
|
|
EXPECT_EQ(indices[1], gidx(0, 1, 2));
|
|
EXPECT_EQ(indices[2], gidx(0, 2, 1));
|
|
EXPECT_EQ(indices[3], gidx(0, 2, 2));
|
|
}
|
|
|
|
// face 1 (x=max), quad 0: fe_idx = 4 (after face 0's 4 quads)
|
|
// g[0] = cx*order = 2
|
|
{
|
|
int indices[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, NULL,
|
|
NULL, NULL, indices, NULL);
|
|
EXPECT_EQ(indices[0], gidx(2, 0, 0));
|
|
EXPECT_EQ(indices[1], gidx(2, 0, 1));
|
|
EXPECT_EQ(indices[2], gidx(2, 1, 0));
|
|
EXPECT_EQ(indices[3], gidx(2, 1, 1));
|
|
}
|
|
}
|
|
|
|
// verify node indices for a non-cubic grid (cx != cz)
|
|
TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) {
|
|
int order = 1;
|
|
int cx = 2, cy = 1, cz = 3;
|
|
int ny_g = cy * order + 1; // 2
|
|
int nz_g = cz * order + 1; // 4
|
|
|
|
// create dummy positions for (2*1+1)*(1*1+1)*(3*1+1) = 3*2*4 = 24 nodes
|
|
std::vector<mjtNum> xpos(3 * 24, 0);
|
|
for (int i = 0; i < 24; i++) {
|
|
int gi = i / 8;
|
|
int gj = (i / 4) % 2;
|
|
int gk = i % 4;
|
|
xpos[3 * i + 0] = gi * 0.1;
|
|
xpos[3 * i + 1] = gj * 0.1;
|
|
xpos[3 * i + 2] = gk * 0.1;
|
|
}
|
|
|
|
auto gidx = [&](int gx, int gy, int gz) {
|
|
return gx * ny_g * nz_g + gy * nz_g + gz;
|
|
};
|
|
|
|
// face 2 (y=0): normal_axis=1, na0=2(z slow), na1=0(x fast)
|
|
// counts: na0 -> cz = 3, na1 -> cx = 2
|
|
// total quads on face 2 = 6
|
|
// we test within_face = 2 (third quad)
|
|
// correct: c1 = cx = 2. q0 = 2/2 = 1, q1 = 2%2 = 0
|
|
//
|
|
// face element index calculation:
|
|
// face 0: cy*cz = 1*3 = 3 quads (indices 0-2)
|
|
// face 1: cy*cz = 1*3 = 3 quads (indices 3-5)
|
|
// face 2: cx*cz = 2*3 = 6 quads. Quad 2 is index 2 within this face.
|
|
// Total flat index = 3 + 3 + 2 = 8
|
|
{
|
|
int indices[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL, NULL,
|
|
NULL, NULL, indices, NULL);
|
|
EXPECT_EQ(indices[0], gidx(0, 0, 1));
|
|
EXPECT_EQ(indices[1], gidx(1, 0, 1));
|
|
EXPECT_EQ(indices[2], gidx(0, 0, 2));
|
|
EXPECT_EQ(indices[3], gidx(1, 0, 2));
|
|
}
|
|
}
|
|
|
|
// verify data gathering: positions, velocities, and reference positions
|
|
TEST_F(FaceStateTest, DataGathering) {
|
|
int order = 1;
|
|
int cx = 1, cy = 1, cz = 1;
|
|
int npe = 4;
|
|
int nnodes = 8;
|
|
|
|
// create positions and velocities for 8 nodes
|
|
std::vector<mjtNum> xpos(3 * nnodes);
|
|
std::vector<mjtNum> vel(3 * nnodes);
|
|
std::vector<mjtNum> xpos0(3 * nnodes);
|
|
for (int i = 0; i < nnodes; i++) {
|
|
for (int d = 0; d < 3; d++) {
|
|
xpos[3 * i + d] = 10 * i + d;
|
|
vel[3 * i + d] = 100 * i + d;
|
|
xpos0[3 * i + d] = 1000 * i + d;
|
|
}
|
|
}
|
|
|
|
// gather face 4 (z=0): nodes at (0,0,0), (0,1,0), (1,0,0), (1,1,0)
|
|
// = global indices 0, 2, 4, 6
|
|
std::vector<mjtNum> xpos_f(3 * npe);
|
|
std::vector<mjtNum> vel_f(3 * npe);
|
|
std::vector<mjtNum> xpos0_f(3 * npe);
|
|
int indices[4];
|
|
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), vel.data(),
|
|
xpos0.data(), xpos_f.data(), vel_f.data(),
|
|
xpos0_f.data(), indices, NULL);
|
|
|
|
for (int n = 0; n < npe; n++) {
|
|
int gi = indices[n];
|
|
for (int d = 0; d < 3; d++) {
|
|
EXPECT_EQ(xpos_f[3 * n + d], xpos[3 * gi + d]);
|
|
EXPECT_EQ(vel_f[3 * n + d], vel[3 * gi + d]);
|
|
EXPECT_EQ(xpos0_f[3 * n + d], xpos0[3 * gi + d]);
|
|
}
|
|
}
|
|
}
|
|
|
|
// verify that flexInterpRotation2D produces identity for axis-aligned faces
|
|
// (tested via mju_flexGatherFaceState with quat output)
|
|
TEST_F(FaceStateTest, IdentityRotationAxisAligned) {
|
|
int order = 1;
|
|
int cx = 1, cy = 1, cz = 1;
|
|
int npe = 4;
|
|
|
|
// create an axis-aligned unit cube: 8 nodes at {0,1}^3
|
|
std::vector<mjtNum> xpos(3 * 8);
|
|
int idx = 0;
|
|
for (int i = 0; i <= 1; i++) {
|
|
for (int j = 0; j <= 1; j++) {
|
|
for (int k = 0; k <= 1; k++) {
|
|
xpos[3 * idx + 0] = i;
|
|
xpos[3 * idx + 1] = j;
|
|
xpos[3 * idx + 2] = k;
|
|
idx++;
|
|
}
|
|
}
|
|
}
|
|
|
|
std::vector<mjtNum> xpos_f(3 * npe);
|
|
mjtNum quat[4];
|
|
|
|
// test all 6 faces: each should give identity rotation (quat = [1,0,0,0])
|
|
int nelem_fe = 6;
|
|
for (int fe = 0; fe < nelem_fe; fe++) {
|
|
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos.data(), NULL, NULL,
|
|
xpos_f.data(), NULL, NULL, NULL, quat);
|
|
EXPECT_NEAR(mju_abs(quat[0]), 1.0, 1e-10) << "face " << fe;
|
|
EXPECT_NEAR(quat[1], 0.0, 1e-10) << "face " << fe;
|
|
EXPECT_NEAR(quat[2], 0.0, 1e-10) << "face " << fe;
|
|
EXPECT_NEAR(quat[3], 0.0, 1e-10) << "face " << fe;
|
|
}
|
|
}
|
|
|
|
// verify that flexInterpRotation2D extracts the correct rotation for a
|
|
// globally rotated cube (90° around z-axis)
|
|
TEST_F(FaceStateTest, RotatedCubeRotation) {
|
|
int order = 1;
|
|
int cx = 1, cy = 1, cz = 1;
|
|
int npe = 4;
|
|
|
|
// create an axis-aligned unit cube, then rotate 90° around z
|
|
// rotation: (x,y,z) → (-y, x, z)
|
|
std::vector<mjtNum> xpos(3 * 8);
|
|
int idx = 0;
|
|
for (int i = 0; i <= 1; i++) {
|
|
for (int j = 0; j <= 1; j++) {
|
|
for (int k = 0; k <= 1; k++) {
|
|
mjtNum orig[3] = {(mjtNum)i, (mjtNum)j, (mjtNum)k};
|
|
mjtNum axis[3] = {0, 0, 1};
|
|
mjtNum rot_quat[4];
|
|
mju_axisAngle2Quat(rot_quat, axis, mjPI / 2);
|
|
mju_rotVecQuat(xpos.data() + 3 * idx, orig, rot_quat);
|
|
idx++;
|
|
}
|
|
}
|
|
}
|
|
|
|
std::vector<mjtNum> xpos_f(3 * npe);
|
|
mjtNum quat[4];
|
|
|
|
// expected rotation: global→local is inverse of the 90° z rotation
|
|
// 90° around z: quat = [cos(45°), 0, 0, sin(45°)]
|
|
// inverse (global→local): [cos(45°), 0, 0, -sin(45°)]
|
|
mjtNum sq2 = mju_sqrt(0.5);
|
|
|
|
// test face 4 (z=0): normal_axis=2, in-plane axes are (0,1)
|
|
// tangent vectors should reflect the 90° z rotation
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
|
|
xpos_f.data(), NULL, NULL, NULL, quat);
|
|
|
|
EXPECT_NEAR(quat[0], sq2, 1e-5);
|
|
EXPECT_NEAR(quat[1], 0.0, 1e-5);
|
|
EXPECT_NEAR(quat[2], 0.0, 1e-5);
|
|
EXPECT_NEAR(quat[3], -sq2, 1e-5);
|
|
|
|
// test face 5 (z=max): should give same rotation
|
|
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL,
|
|
xpos_f.data(), NULL, NULL, NULL, quat);
|
|
|
|
EXPECT_NEAR(quat[0], sq2, 1e-5);
|
|
EXPECT_NEAR(quat[1], 0.0, 1e-5);
|
|
EXPECT_NEAR(quat[2], 0.0, 1e-5);
|
|
EXPECT_NEAR(quat[3], -sq2, 1e-5);
|
|
}
|
|
|
|
// verify that flexInterpRotation2D matches the 3D cell rotation for
|
|
// the same globally-rotated cube
|
|
TEST_F(FaceStateTest, RotationConsistencyWith3D) {
|
|
int order = 1;
|
|
int cx = 1, cy = 1, cz = 1;
|
|
|
|
// create 90° z-rotated unit cube
|
|
std::vector<mjtNum> xpos(3 * 8);
|
|
int idx = 0;
|
|
for (int i = 0; i <= 1; i++) {
|
|
for (int j = 0; j <= 1; j++) {
|
|
for (int k = 0; k <= 1; k++) {
|
|
mjtNum orig[3] = {(mjtNum)i, (mjtNum)j, (mjtNum)k};
|
|
mjtNum axis[3] = {0, 0, 1};
|
|
mjtNum rot_quat[4];
|
|
mju_axisAngle2Quat(rot_quat, axis, mjPI / 6);
|
|
mju_rotVecQuat(xpos.data() + 3 * idx, orig, rot_quat);
|
|
idx++;
|
|
}
|
|
}
|
|
}
|
|
|
|
// get 3D cell rotation
|
|
int npc = 8;
|
|
std::vector<mjtNum> xpos_c(3 * npc);
|
|
mjtNum quat_3d[4];
|
|
mju_flexGatherCellState(order, cy, cz, 0, 0, 0, xpos.data(), NULL, NULL,
|
|
xpos_c.data(), NULL, NULL, NULL, quat_3d);
|
|
|
|
// get 2D face rotation for each face and verify it matches the 3D rotation
|
|
int npe = 4;
|
|
std::vector<mjtNum> xpos_f(3 * npe);
|
|
|
|
int nelem_fe = 6;
|
|
for (int fe = 0; fe < nelem_fe; fe++) {
|
|
mjtNum quat_2d[4];
|
|
mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos.data(), NULL, NULL,
|
|
xpos_f.data(), NULL, NULL, NULL, quat_2d);
|
|
|
|
// quaternions may differ by sign; compare unsigned
|
|
mjtNum dot = quat_3d[0] * quat_2d[0] + quat_3d[1] * quat_2d[1] +
|
|
quat_3d[2] * quat_2d[2] + quat_3d[3] * quat_2d[3];
|
|
EXPECT_NEAR(mju_abs(dot), 1.0, 1e-5)
|
|
<< "face " << fe << ": 2D rotation differs from 3D cell rotation";
|
|
}
|
|
}
|
|
|
|
// ------------------------------ Shell TFI Interpolation ----------------------
|
|
|
|
using ShellTFITest = MujocoTest;
|
|
|
|
// helper: set up a regular nx*ny*nz grid with positions at grid indices
|
|
static void MakeRegularGrid(mjtNum* nodexpos, int nx, int ny, int nz) {
|
|
for (int i = 0; i < nx; i++) {
|
|
for (int j = 0; j < ny; j++) {
|
|
for (int k = 0; k < nz; k++) {
|
|
int idx = i * ny * nz + j * nz + k;
|
|
nodexpos[3 * idx + 0] = (mjtNum)i;
|
|
nodexpos[3 * idx + 1] = (mjtNum)j;
|
|
nodexpos[3 * idx + 2] = (mjtNum)k;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST_F(ShellTFITest, IdentityGrid) {
|
|
// 3x3x3 grid: 1 interior node at (1,1,1)
|
|
constexpr int nx = 3, ny = 3, nz = 3;
|
|
mjtNum nodexpos[3 * nx * ny * nz];
|
|
MakeRegularGrid(nodexpos, nx, ny, nz);
|
|
|
|
// save expected interior position
|
|
mjtNum expected[3] = {1.0, 1.0, 1.0};
|
|
|
|
// run TFI
|
|
mju_shellTrackInterior(nodexpos, nx, ny, nz);
|
|
|
|
// interior node at (1,1,1) should match
|
|
int idx = 1 * ny * nz + 1 * nz + 1;
|
|
EXPECT_NEAR(nodexpos[3 * idx + 0], expected[0], MjTol(1e-12, 1e-5));
|
|
EXPECT_NEAR(nodexpos[3 * idx + 1], expected[1], MjTol(1e-12, 1e-5));
|
|
EXPECT_NEAR(nodexpos[3 * idx + 2], expected[2], MjTol(1e-12, 1e-5));
|
|
}
|
|
|
|
TEST_F(ShellTFITest, UniformScaling) {
|
|
// 3x3x3: scale all boundary nodes by 2x, interior should follow
|
|
constexpr int nx = 3, ny = 3, nz = 3;
|
|
mjtNum nodexpos[3 * nx * ny * nz];
|
|
MakeRegularGrid(nodexpos, nx, ny, nz);
|
|
|
|
// scale all nodes
|
|
for (int i = 0; i < 3 * nx * ny * nz; i++) {
|
|
nodexpos[i] *= 2.0;
|
|
}
|
|
|
|
// run TFI — interior should be reconstructed to 2*original
|
|
mju_shellTrackInterior(nodexpos, nx, ny, nz);
|
|
|
|
int idx = 1 * ny * nz + 1 * nz + 1;
|
|
EXPECT_NEAR(nodexpos[3 * idx + 0], 2.0, MjTol(1e-12, 1e-5));
|
|
EXPECT_NEAR(nodexpos[3 * idx + 1], 2.0, MjTol(1e-12, 1e-5));
|
|
EXPECT_NEAR(nodexpos[3 * idx + 2], 2.0, MjTol(1e-12, 1e-5));
|
|
}
|
|
|
|
TEST_F(ShellTFITest, AffineDeformation) {
|
|
// 4x4x4 grid with 8 interior nodes. Apply affine transform to boundary,
|
|
// then verify TFI reproduces the same affine transform on interior nodes.
|
|
constexpr int nx = 4, ny = 4, nz = 4;
|
|
mjtNum nodexpos[3 * nx * ny * nz];
|
|
MakeRegularGrid(nodexpos, nx, ny, nz);
|
|
|
|
// affine: F(x,y,z) = A*[x,y,z]^T + b
|
|
// A = [[2, 0.5, 0], [0.3, 1.5, 0], [0, 0, 1]], b = [10, 20, 30]
|
|
auto affine = [](mjtNum x, mjtNum y, mjtNum z, mjtNum out[3]) {
|
|
out[0] = 2.0 * x + 0.5 * y + 10.0;
|
|
out[1] = 0.3 * x + 1.5 * y + 20.0;
|
|
out[2] = z + 30.0;
|
|
};
|
|
|
|
// apply affine to all nodes
|
|
for (int i = 0; i < nx; i++) {
|
|
for (int j = 0; j < ny; j++) {
|
|
for (int k = 0; k < nz; k++) {
|
|
int idx = i * ny * nz + j * nz + k;
|
|
affine((mjtNum)i, (mjtNum)j, (mjtNum)k, nodexpos + 3 * idx);
|
|
}
|
|
}
|
|
}
|
|
|
|
// corrupt interior nodes to verify TFI actually reconstructs them
|
|
for (int i = 1; i < nx - 1; i++) {
|
|
for (int j = 1; j < ny - 1; j++) {
|
|
for (int k = 1; k < nz - 1; k++) {
|
|
int idx = i * ny * nz + j * nz + k;
|
|
nodexpos[3 * idx + 0] = -999;
|
|
nodexpos[3 * idx + 1] = -999;
|
|
nodexpos[3 * idx + 2] = -999;
|
|
}
|
|
}
|
|
}
|
|
|
|
// run TFI
|
|
mju_shellTrackInterior(nodexpos, nx, ny, nz);
|
|
|
|
// check all interior nodes match affine
|
|
for (int i = 1; i < nx - 1; i++) {
|
|
for (int j = 1; j < ny - 1; j++) {
|
|
for (int k = 1; k < nz - 1; k++) {
|
|
int idx = i * ny * nz + j * nz + k;
|
|
mjtNum expected[3];
|
|
affine((mjtNum)i, (mjtNum)j, (mjtNum)k, expected);
|
|
EXPECT_NEAR(nodexpos[3 * idx + 0], expected[0], MjTol(1e-12, 1e-4))
|
|
<< "i=" << i << " j=" << j << " k=" << k;
|
|
EXPECT_NEAR(nodexpos[3 * idx + 1], expected[1], MjTol(1e-12, 1e-4))
|
|
<< "i=" << i << " j=" << j << " k=" << k;
|
|
EXPECT_NEAR(nodexpos[3 * idx + 2], expected[2], MjTol(1e-12, 1e-4))
|
|
<< "i=" << i << " j=" << j << " k=" << k;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST_F(ShellTFITest, BoundaryUnmodified) {
|
|
// verify that boundary nodes are not modified by TFI
|
|
constexpr int nx = 4, ny = 4, nz = 4;
|
|
mjtNum nodexpos[3 * nx * ny * nz];
|
|
MakeRegularGrid(nodexpos, nx, ny, nz);
|
|
|
|
// save boundary node values
|
|
mjtNum saved[3 * nx * ny * nz];
|
|
mju_copy(saved, nodexpos, 3 * nx * ny * nz);
|
|
|
|
mju_shellTrackInterior(nodexpos, nx, ny, nz);
|
|
|
|
// check all boundary nodes unchanged
|
|
for (int i = 0; i < nx; i++) {
|
|
for (int j = 0; j < ny; j++) {
|
|
for (int k = 0; k < nz; k++) {
|
|
bool is_boundary = (i == 0 || i == nx - 1 || j == 0 || j == ny - 1 ||
|
|
k == 0 || k == nz - 1);
|
|
if (is_boundary) {
|
|
int idx = i * ny * nz + j * nz + k;
|
|
EXPECT_EQ(nodexpos[3 * idx + 0], saved[3 * idx + 0]);
|
|
EXPECT_EQ(nodexpos[3 * idx + 1], saved[3 * idx + 1]);
|
|
EXPECT_EQ(nodexpos[3 * idx + 2], saved[3 * idx + 2]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST_F(ShellTFITest, NoInteriorSmallGrid) {
|
|
// 2x2x2 and 2x3x2: no interior nodes, TFI should be a no-op
|
|
constexpr int nx = 2, ny = 3, nz = 2;
|
|
mjtNum nodexpos[3 * nx * ny * nz];
|
|
MakeRegularGrid(nodexpos, nx, ny, nz);
|
|
|
|
mjtNum saved[3 * nx * ny * nz];
|
|
mju_copy(saved, nodexpos, 3 * nx * ny * nz);
|
|
|
|
mju_shellTrackInterior(nodexpos, nx, ny, nz);
|
|
|
|
// all nodes unchanged
|
|
for (int i = 0; i < 3 * nx * ny * nz; i++) {
|
|
EXPECT_EQ(nodexpos[i], saved[i]);
|
|
}
|
|
}
|
|
|
|
TEST_F(UtilMiscTest, Round) {
|
|
// basic rounding
|
|
EXPECT_EQ(mju_round(2.3), 2);
|
|
EXPECT_EQ(mju_round(2.7), 3);
|
|
EXPECT_EQ(mju_round(-2.3), -2);
|
|
EXPECT_EQ(mju_round(-2.7), -3);
|
|
|
|
// exact integers
|
|
EXPECT_EQ(mju_round(0.0), 0);
|
|
EXPECT_EQ(mju_round(3.0), 3);
|
|
EXPECT_EQ(mju_round(-3.0), -3);
|
|
|
|
// ties: round() rounds away from zero
|
|
EXPECT_EQ(mju_round(0.5), 1);
|
|
EXPECT_EQ(mju_round(1.5), 2);
|
|
EXPECT_EQ(mju_round(-0.5), -1);
|
|
EXPECT_EQ(mju_round(-1.5), -2);
|
|
|
|
// overflow clamps to INT_MAX/INT_MIN
|
|
EXPECT_EQ(mju_round(1e18), INT_MAX);
|
|
EXPECT_EQ(mju_round(-1e18), INT_MIN);
|
|
}
|
|
|
|
} // namespace
|
|
} // namespace mujoco
|