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

Enable float32 testing for most MuJoCo engine and user tests.

Browse Source 
PiperOrigin-RevId: 886697701
Change-Id: I4a96fae03ea18494c3fcef8eb17b3b6f0863e9b7
This commit is contained in:
Yuval Tassa
2026-03-20 04:04:34 -07:00
committed by Copybara-Service
parent cab4b73f41
commit 9fa3f6f77e
28 changed files with 536 additions and 423 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_collision_box_test.cc
+3 -3
View File
@@ -30,7 +30,6 @@ namespace {
using MjCollisionBoxTest = MujocoTest;
using ::testing::NotNull;
using ::testing::DoubleNear;
static const char* const kBad0FilePath =
"engine/testdata/collision_box/boxbox_bad0.xml";
@@ -266,7 +265,8 @@ TEST_F(MjCollisionBoxTest, BoxSphere) {
data->qpos[2] = z;
mj_forward(model, data);
EXPECT_EQ(data->ncon, 2);
EXPECT_THAT(data->contact[0].dist, DoubleNear(data->contact[1].dist, 1e-8));
EXPECT_THAT(data->contact[0].dist,
MjNear(data->contact[1].dist, 1e-8, 1e-6));
}
mj_deleteData(data);
@@ -294,7 +294,7 @@ TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
for (mjfCollision collision : {mjc_BoxBox, mjc_Convex}) {
int n = collision(model, data, contact, 0, 1, 0.0);
for (int i = 0; i < n; i++) {
EXPECT_NEAR(contact[i].dist, -0.5, 1.0e-8);
EXPECT_NEAR(contact[i].dist, -0.5, MjTol(1e-8, 1e-6));
}
}
test/engine/engine_collision_sdf_test.cc
+4 -2
View File
@@ -59,7 +59,8 @@ TEST_F(SdfTest, SdfPrimitive) {
mjtNum gradient[3], dist[kgeoms][kpoints] = {
{0, 0, 0, 0, 1, 1}, // plane
{-1, 0, 0, mju_sqrt(2)-1, mju_sqrt(2)-1, mju_sqrt(3)-1}, // sphere
{-.1, .9, .9, mju_sqrt(2)-.1, .9, mju_sqrt(2)-.1}, // capsule
{(mjtNum)-.1, (mjtNum).9, (mjtNum).9,
mju_sqrt(2)-(mjtNum).1, (mjtNum).9, mju_sqrt(2)-(mjtNum).1}, // capsule
{-1, 0, 0, mju_sqrt(2)-1, 0, mju_sqrt(2)-1}, // cylinder
{-mju_sqrt(3), 0, 0, 0, 0, 0}, // box
};
@@ -72,7 +73,8 @@ TEST_F(SdfTest, SdfPrimitive) {
sdf.type = mjSDFTYPE_SINGLE;
sdf.geomtype = (mjtGeom*)(model->geom_type+i);
for (int j = 0; j < kpoints; j++) {
EXPECT_NEAR(mjc_distance(model, data, &sdf, points[j]), dist[i][j], 1e-9);
EXPECT_NEAR(mjc_distance(model, data, &sdf, points[j]), dist[i][j],
MjTol(1e-9, 5e-7));
mjc_gradient(model, data, &sdf, gradient, points[j]);
}
}
test/engine/engine_core_constraint_test.cc
+15 -10
View File
@@ -33,7 +33,6 @@
namespace mujoco {
namespace {
using ::testing::DoubleNear;
using ::testing::NotNull;
using ::testing::Pointwise;
using CoreConstraintTest = MujocoTest;
@@ -63,6 +62,9 @@ void RotationResidual(const mjModel *model, mjData *data,
// validate rotational Jacobian used in welds
TEST_F(CoreConstraintTest, WeldRotJacobian) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "FD Jacobian with eps=1e-6 below float32 precision";
#endif
constexpr char xml[] = R"(
<mujoco>
<option jacobian="dense"/>
@@ -157,7 +159,7 @@ TEST_F(CoreConstraintTest, WeldRotJacobian) {
// test that analytical and finite-differenced Jacobians match
EXPECT_THAT(AsVector(jacFD, 3*nv),
Pointwise(DoubleNear(eps), AsVector(jacdif, 3*nv)));
Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3*nv)));
mj_deleteData(data);
mj_deleteModel(model);
@@ -214,7 +216,7 @@ TEST_F(CoreConstraintTest, RestPenetration) {
expected_depth = gravity * (1 - impedance) * tc_dr * tc_dr;
}
EXPECT_THAT(depth, DoubleNear(expected_depth, 1e-10));
EXPECT_THAT(depth, MjNear(expected_depth, 1e-10, 1e-3));
}
}
@@ -291,7 +293,7 @@ TEST_F(CoreConstraintTest, EqualityBodySite) {
// compare
EXPECT_EQ(nefc_site, data->nefc);
EXPECT_THAT(AsVector(data->efc_diagApprox, data->nefc),
Pointwise(DoubleNear(1e-12), dA));
Pointwise(MjNear(1e-12, 1e-4), dA));
mj_deleteData(data);
mj_deleteModel(model);
@@ -386,7 +388,7 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
int i = map2efc[c];
EXPECT_EQ(d2->efc_island[i], island);
EXPECT_EQ(state[c], d1->efc_state[i]);
EXPECT_THAT(force[c], DoubleNear(d1->efc_force[i], 1e-12));
EXPECT_THAT(force[c], MjNear(d1->efc_force[i], 1e-12, 1e-4));
}
// compare cone Hessians
@@ -397,7 +399,7 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
d2->efc_state[efcadr] == mjCNSTRSTATE_CONE) {
for (int j=0; j < 36; j++) {
EXPECT_THAT(d2->contact[c].H[j],
DoubleNear(d1->contact[c].H[j], 1e-12));
MjNear(d1->contact[c].H[j], 1e-12, 1e-4));
}
}
}
@@ -410,7 +412,7 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
}
// expect monolithic total cost
EXPECT_THAT(cost1, DoubleNear(cost2, 1e-12));
EXPECT_THAT(cost1, MjNear(cost2, 1e-12, 1e-4));
mju_free(jar);
}
@@ -449,7 +451,7 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
EXPECT_EQ(data->nefc, 18);
for (int i = 0; i < 18; ++i) {
EXPECT_EQ(data->efc_type[i], mjCNSTR_EQUALITY);
EXPECT_NEAR(data->efc_pos[i], 0, 1e-9);
EXPECT_NEAR(data->efc_pos[i], 0, MjTol(1e-9, 1e-5));
}
// check that efc_J has rigid-body motions in kernel
@@ -464,7 +466,7 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
}
mj_mulJacVec(model, data, Jqvel.data(), qvel.data());
for (int j = 0; j < data->nefc; ++j) {
EXPECT_NEAR(Jqvel[j], 0, 1e-9);
EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5));
}
}
@@ -483,7 +485,7 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
}
mj_mulJacVec(model, data, Jqvel.data(), qvel.data());
for (int j = 0; j < data->nefc; ++j) {
EXPECT_NEAR(Jqvel[j], 0, 1e-9);
EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5));
}
}
@@ -493,6 +495,9 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
// Test flex strain constraint with pinned nodes attached to freejoint parent
TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "FD Jacobian mismatch due to float32 precision";
#endif
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" jacobian="dense" gravity="0 0 0"/>
test/engine/engine_core_smooth_test.cc
+37 -43
View File
@@ -35,14 +35,13 @@
namespace mujoco {
namespace {
using ::std::string;
using ::std::vector;
using ::testing::Each;
using ::testing::ElementsAre;
using ::testing::Eq;
using ::testing::Pointwise;
using ::testing::DoubleNear;
using ::testing::NotNull;
using ::testing::Not;
using ::testing::NotNull;
using ::testing::Pointwise;
using CoreSmoothTest = MujocoTest;
@@ -81,7 +80,7 @@ TEST_F(CoreSmoothTest, MjDataWorldBodyValuesAreInitialized) {
} else if constexpr (EndsWith(#name, "mat")) { \
EXPECT_THAT(values, ElementsAre(1, 0, 0, 0, 1, 0, 0, 0, 1)) << #name; \
} else if constexpr (std::string_view(#type) == "mjtNum") { \
EXPECT_THAT(values, Each(Eq(0))) << #name; \
EXPECT_THAT(values, Each(MjNear(0.0, 1e-7, 1e-7))) << #name; \
} \
}
MJDATA_POINTERS
@@ -198,7 +197,7 @@ TEST_F(CoreSmoothTest, TendonJdot) {
mj_tendonDot(m, d, 0, ten_Jdot.data());
// compute finite-differenced Jdot
mjtNum h = 1e-7;
mjtNum h = MjTol(1e-7, 5e-4);
mj_integratePos(m, d->qpos, d->qvel, h);
mj_kinematics(m, d);
mj_comPos(m, d);
@@ -207,9 +206,7 @@ TEST_F(CoreSmoothTest, TendonJdot) {
mju_subFrom(ten_Jh.data(), ten_J.data(), nv);
mju_scl(ten_Jh.data(), ten_Jh.data(), 1.0 / h, nv);
// expect analytic and FD derivatives to be similar to eps precision
mjtNum eps = 1e-6;
EXPECT_THAT(ten_Jdot, Pointwise(DoubleNear(eps), ten_Jh));
EXPECT_THAT(ten_Jdot, Pointwise(MjNear(1e-6, 2e-3), ten_Jh));
}
mj_deleteData(d);
@@ -266,7 +263,7 @@ TEST_F(CoreSmoothTest, TendonArmature) {
}
// expect matrices to match
EXPECT_THAT(M2, Pointwise(DoubleNear(1e-9), M));
EXPECT_THAT(M2, Pointwise(MjNear(1e-9, 1e-5), M));
}
mj_deleteData(d);
@@ -305,8 +302,8 @@ TEST_F(CoreSmoothTest, TendonArmatureConservesEnergy) {
mjtNum time = d->time;
mj_step(m, d);
ASSERT_GT(d->time, time) << "Divergence detected";
double energy_t = d->energy[0] + d->energy[1];
EXPECT_THAT(energy_t, DoubleNear(energy_0, eps));
mjtNum energy_t = d->energy[0] + d->energy[1];
EXPECT_NEAR(energy_t, energy_0, MjTol(eps, 3*eps));
}
}
mj_deleteData(d);
@@ -329,15 +326,13 @@ TEST_F(CoreSmoothTest, TendonArmatureConservesMomentum) {
// this model contains subtreelinvel and subtreeangmom sensors
vector<mjtNum> sdata_0 = AsVector(d->sensordata, m->nsensordata);
EXPECT_THAT(sdata_0, Each(Eq(0)));
double eps = 1e-5;
EXPECT_THAT(sdata_0, Each(MjNear(0.0, 1e-15, 1e-7)));
while (d->time < 1) {
mjtNum time = d->time;
mj_step(m, d);
ASSERT_GT(d->time, time) << "Divergence detected";
vector<mjtNum> sdata_t = AsVector(d->sensordata, m->nsensordata);
EXPECT_THAT(sdata_t, Pointwise(DoubleNear(eps), sdata_0));
EXPECT_THAT(sdata_t, Pointwise(MjNear(1e-5, 5e-4), sdata_0));
}
// momentum is conserved nontrivially (velocities are non-zero)
@@ -388,8 +383,7 @@ TEST_F(CoreSmoothTest, TendonInertiaEquivalent) {
mj_step(m_e, d_e);
ASSERT_GT(d_e->time, time) << "Divergence detected";
vector<mjtNum> xpos_e = AsVector(d_e->geom_xpos + 3*gid_e, 3);
EXPECT_THAT(xpos, Pointwise(DoubleNear(eps), xpos_e));
EXPECT_THAT(xpos, Pointwise(MjNear(eps, 10*eps), xpos_e));
}
mj_deleteData(d);
mj_deleteModel(m);
@@ -412,7 +406,8 @@ void TestConnect(const char* const filepath) {
mj_step(model, data);
}
for (int i=0; i < 3; i++) {
EXPECT_NEAR(data->sensordata[i], model->sensor_user[i], 1e-6);
EXPECT_THAT(data->sensordata[i] - model->sensor_user[i],
MjNear(0, 1e-6, 1e-4));
}
mj_deleteData(data);
mj_deleteModel(model);
@@ -471,7 +466,7 @@ void TestWeld(const char* const filepath) {
EXPECT_NEAR(
data->sensordata[model->sensor_adr[sensor_index] + i],
model->sensor_user[model->nuser_sensor*sensor_index + i],
1e-6);
MjTol(1e-6, 5e-5));
}
}
mj_deleteData(data);
@@ -570,7 +565,7 @@ TEST_F(CoreSmoothTest, EqualityBodySite) {
// compare
EXPECT_THAT(AsVector(data->sensordata, model->nsensordata),
Pointwise(DoubleNear(1e-8), sdata));
Pointwise(MjNear(1e-8, 5e-4), sdata));
mj_deleteData(data);
mj_deleteModel(model);
@@ -604,18 +599,16 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
int site_id = mj_name2id(model, mjOBJ_SITE, "end_effector");
// check that position matches target to within 1e-3 length units
double tol_pos = 1e-3;
mjtNum relpos[3];
mju_sub3(relpos, data->site_xpos+3*site_id, data->site_xpos+3*refsite_id);
EXPECT_THAT(relpos, Pointwise(DoubleNear(tol_pos), targetpos));
EXPECT_THAT(relpos, Pointwise(MjNear(1e-3, 5e-3), targetpos));
// check that orientation matches target to within 0.06 radians
double tol_rot = 0.06;
mjtNum site_xquat[4], refsite_xquat[4], relrot[3];
mju_mat2Quat(refsite_xquat, data->site_xmat+9*refsite_id);
mju_mat2Quat(site_xquat, data->site_xmat+9*site_id);
mju_subQuat(relrot, site_xquat, refsite_xquat);
EXPECT_THAT(relrot, Pointwise(DoubleNear(tol_rot), targetrot));
EXPECT_THAT(relrot, Pointwise(MjNear(0.06, 0.06), targetrot));
mj_deleteData(data);
mj_deleteModel(model);
@@ -633,7 +626,7 @@ TEST_F(CoreSmoothTest, RefsiteConservesMomentum) {
data->ctrl[1] = -1;
// simulate, assert that momentum is conserved
mjtNum eps = 1e-9;
mjtNum eps = MjTol(1e-9, 2e-6);
while (data->time < 1) {
mjtNum time = data->time;
mj_step(model, data);
@@ -690,7 +683,7 @@ TEST_F(CoreSmoothTest, FactorI) {
mj_fullM(model, Mexpected.data(), data->qM);
// expect matrices to match to floating point precision
EXPECT_THAT(M, Pointwise(DoubleNear(1e-12), Mexpected));
EXPECT_THAT(M, Pointwise(MjNear(1e-12, 1e-5), Mexpected));
mj_deleteData(data);
mj_deleteModel(model);
@@ -723,7 +716,7 @@ TEST_F(CoreSmoothTest, SolveLDs) {
// expect lower triangles to match exactly
for (int i=0; i < nv; i++) {
for (int j=0; j < i; j++) {
EXPECT_EQ(LDdense[i*nv+j], LDdense2[i*nv+j]);
EXPECT_NEAR(LDdense[i*nv+j], LDdense2[i*nv+j], MjTol(1e-14, 1e-6));
}
}
@@ -739,7 +732,7 @@ TEST_F(CoreSmoothTest, SolveLDs) {
// expect vectors to match up to floating point precision
for (int i=0; i < nv; i++) {
EXPECT_FLOAT_EQ(vec[i], vec2[i]);
EXPECT_NEAR(vec[i], vec2[i], MjTol(1e-14, 5e-6));
}
mj_deleteData(d);
@@ -774,7 +767,7 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) {
// expect vectors to match up to floating point precision
for (int i=0; i < nv*n; i++) {
EXPECT_FLOAT_EQ(vec[i], vec2[i]);
EXPECT_NEAR(vec[i], vec2[i], MjTol(1e-14, 5e-6));
}
mj_deleteData(d);
@@ -812,8 +805,9 @@ TEST_F(CoreSmoothTest, SolveM2) {
// expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v)
for (int i=0; i < n; i++) {
EXPECT_FLOAT_EQ(mju_dot(vec2.data() + i*nv, vec.data() + i*nv, nv),
mju_dot(res.data() + i*nv, res.data() + i*nv, nv));
EXPECT_NEAR(mju_dot(vec2.data() + i*nv, vec.data() + i*nv, nv),
mju_dot(res.data() + i*nv, res.data() + i*nv, nv),
MjTol(1e-10, 1e-2));
}
mj_deleteData(d);
@@ -850,8 +844,8 @@ TEST_F(CoreSmoothTest, FactorIs) {
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
// expect outputs to match to floating point precision
EXPECT_THAT(qLD, Pointwise(DoubleNear(1e-12), qLDexpected));
EXPECT_THAT(qLDiagInv, Pointwise(DoubleNear(1e-12), qLDiagInvExpected));
EXPECT_THAT(qLD, Pointwise(MjNear(1e-12, 1e-4), qLDexpected));
EXPECT_THAT(qLDiagInv, Pointwise(MjNear(1e-12, 1e-4), qLDiagInvExpected));
/* uncomment for debugging
vector<mjtNum> LDdense(nv*nv);
@@ -900,8 +894,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
int nvert = m->flex_vertnum[0];
ASSERT_EQ(nvert, 9);
for (int i=0; i < nvert; i++) {
EXPECT_NEAR(d->flexvert_length[2*i+0], 0.0, 1e-5);
EXPECT_NEAR(d->flexvert_length[2*i+1], 0.0, 1e-5);
EXPECT_NEAR(d->flexvert_length[2*i+0], 0.0, MjTol(1e-5, 5e-5));
EXPECT_NEAR(d->flexvert_length[2*i+1], 0.0, MjTol(1e-5, 5e-5));
}
// set qvel to rigid rotation
@@ -924,7 +918,7 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
d->qvel[m->flexvert_J_colind[row_start + j]];
}
}
EXPECT_THAT(Jv, Each(DoubleNear(0.0, 1e-9)));
EXPECT_THAT(Jv, Each(MjNear(0.0, 1e-7, 1e-4)));
// check sparsity pattern
int corners[] = {0, 2, 6, 8};
@@ -972,7 +966,7 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
mj_kinematics(m, d);
mj_flex(m, d);
mjtNum eps = 1e-7;
mjtNum eps = MjTol(1e-6, 1e-4);
int nflexvert = m->flex_vertnum[0];
std::vector<mjtNum> jac_fd(2 * nflexvert * m->nv);
std::vector<mjtNum> qpos_backup(m->nq);
@@ -1021,15 +1015,15 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
d->flexvert_J[row_start+i];
}
}
EXPECT_THAT(jac_analytic, Not(Each(Eq(0))));
EXPECT_THAT(jac_analytic, Pointwise(DoubleNear(tolerance), jac_fd));
EXPECT_THAT(jac_analytic, Not(Each(MjNear(0.0, 1e-7, 1e-4))));
EXPECT_THAT(jac_analytic, Pointwise(MjNear(tolerance, 1e-1), jac_fd));
mju_copy(d->qpos, qpos0.data(), m->nq);
mj_kinematics(m, d);
mj_flex(m, d);
};
fd_check(5e-5);
fd_check(MjTol(5e-5, 5e-2));
// Set qpos to put flex in scale=2 configuration.
for (int i=0; i < nvert; i++) {
@@ -1053,8 +1047,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
// Invariant 1: Det(C) - 1 = 4 * 4 - 1 = 15
// Note: constraints are now scaled by sqrt(mass)
for (int i=0; i < nvert; i++) {
EXPECT_NEAR(d->flexvert_length[2 * i + 0], 6.0 * scale, 1e-5);
EXPECT_NEAR(d->flexvert_length[2 * i + 1], 15.0 * scale, 1e-5);
EXPECT_NEAR(d->flexvert_length[2 * i + 0], 6.0 * scale, MjTol(1e-5, 5e-4));
EXPECT_NEAR(d->flexvert_length[2 * i + 1], 15.0 * scale, MjTol(1e-5, 5e-4));
}
// Perturb z-positions so configuration is not flat
test/engine/engine_derivative_test.cc
+27 -26
View File
@@ -44,7 +44,11 @@ using ::testing::NotNull;
using DerivativeTest = MujocoTest;
// errors smaller than this are ignored
static const mjtNum absolute_tolerance = 1e-9;
#ifdef mjUSESINGLE
static const mjtNum absolute_tolerance = 1e-3;
#else
static const mjtNum absolute_tolerance = 1e-9;
#endif
// corrected relative error
static mjtNum RelativeError(mjtNum a, mjtNum b) {
@@ -124,7 +128,7 @@ TEST_F(DerivativeTest, SmoothDvel) {
vector<mjtNum> qDerivAnalytic = AsVector(data->qDeriv, nD);
// compute finite-difference derivatives
mjtNum eps = 1e-7;
mjtNum eps = MjTol(1e-7, 1e-3);
mju_zero(data->qDeriv, nD);
mjd_smooth_velFD(model, data, eps);
@@ -134,7 +138,7 @@ TEST_F(DerivativeTest, SmoothDvel) {
// expect FD and analytic derivatives to be similar to eps precision
EXPECT_THAT(AsVector(data->qDeriv, nD),
Pointwise(DoubleNear(eps), qDerivAnalytic));
Pointwise(MjNear(1e-7, 3e-3), qDerivAnalytic));
}
mj_deleteData(data);
mj_deleteModel(model);
@@ -316,13 +320,12 @@ TEST_F(DerivativeTest, PassiveDvel) {
// clear qDeriv, get finite-difference derivatives
mju_zero(data->qDeriv, nD);
mju_zero(qDerivFD, nD);
mjtNum eps = 1e-6;
mjtNum eps = MjTol(1e-6, 1e-3);
mjd_passive_velFD(model, data, eps);
// expect FD and analytic derivatives to be similar to tol precision
mjtNum tol = 1e-4;
EXPECT_THAT(AsVector(data->qDeriv, nD),
Pointwise(DoubleNear(tol), AsVector(qDerivAnalytic, nD)));
Pointwise(MjNear(1e-4, 1e-3), AsVector(qDerivAnalytic, nD)));
}
mju_free(qDerivFD);
@@ -502,7 +505,7 @@ TEST_F(DerivativeTest, LinearSystem) {
// PrintMatrix(B, 2*nv, nu);
// forward differenced A and B
mjtNum eps = 1e-6;
mjtNum eps = MjTol(1e-6, 1e-3);
mjtNum* AFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*2*nv);
mjtNum* BFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*nu);
@@ -557,7 +560,7 @@ TEST_F(DerivativeTest, ClampedCtrlDerivatives) {
LinearSystem(model, data, nullptr, B);
// forward differenced A and B
mjtNum eps = 1e-6;
mjtNum eps = MjTol(1e-6, 1e-3);
mjtNum* BFD = (mjtNum*) mju_malloc(sizeof(mjtNum)*2*nv*nu);
// set ctrl to the limits, request forward differences
@@ -782,10 +785,9 @@ TEST_F(DerivativeTest, DenseSparseRneEquivalent) {
mjd_passive_vel(model, data);
mjd_rne_vel_dense(model, data);
// expect dense and sparse derivatives to be similar to eps precision
mjtNum eps = 1e-12;
// expect dense and sparse derivatives to be similar to precision
EXPECT_THAT(AsVector(data->qDeriv, nD),
Pointwise(DoubleNear(eps), AsVector(qDeriv, nD)));
Pointwise(MjNear(1e-12, 5e-5), AsVector(qDeriv, nD)));
mj_deleteData(data);
mju_free(qDeriv);
@@ -937,7 +939,7 @@ static void subQuatFD(mjtNum Da[9], mjtNum Db[9],
TEST_F(DerivativeTest, SubQuat) {
const int nrepeats = 10; // number of repeats
const mjtNum eps = 1e-7; // epsilon for finite-differencing and comparison
const mjtNum eps = MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison
int seed = 1;
for (int i = 0; i < nrepeats; i++) {
@@ -961,9 +963,9 @@ TEST_F(DerivativeTest, SubQuat) {
// expect numerical equality
EXPECT_THAT(AsVector(DaFD, 9),
Pointwise(DoubleNear(eps), AsVector(Da, 9)));
Pointwise(MjNear(1e-7, 1e-3), AsVector(Da, 9)));
EXPECT_THAT(AsVector(DbFD, 9),
Pointwise(DoubleNear(eps), AsVector(Db, 9)));
Pointwise(MjNear(1e-7, 1e-3), AsVector(Db, 9)));
}
}
}
@@ -1046,7 +1048,7 @@ void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9],
TEST_F(DerivativeTest, quatIntegrate) {
const int nrepeats = 10; // number of repeats
const mjtNum eps = 1e-7; // epsilon for finite-differencing and comparison
const mjtNum eps = MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison
int seed = 1;
for (int i = 0; i < nrepeats; i++) {
@@ -1070,12 +1072,12 @@ TEST_F(DerivativeTest, quatIntegrate) {
mjd_quatIntegrateFD(DquatFD, DsFD, DvelFD, DhFD, quat, vel, h, eps);
// expect numerical equality of un/scaled velocity derivatives
EXPECT_THAT(AsVector(DvelFD, 9), Pointwise(DoubleNear(eps), DsFD));
EXPECT_THAT(AsVector(DvelFD, 9), Pointwise(MjNear(1e-7, 1e-3), DsFD));
// expect numerical equality of analytic and FD derivatives
EXPECT_THAT(AsVector(DquatFD, 9), Pointwise(DoubleNear(eps), Dquat));
EXPECT_THAT(AsVector(DvelFD, 9), Pointwise(DoubleNear(eps), Dvel));
EXPECT_THAT(AsVector(DhFD, 3), Pointwise(DoubleNear(eps), Dh));
EXPECT_THAT(AsVector(DquatFD, 9), Pointwise(MjNear(1e-7, 1e-3), Dquat));
EXPECT_THAT(AsVector(DvelFD, 9), Pointwise(MjNear(1e-7, 1e-3), Dvel));
EXPECT_THAT(AsVector(DhFD, 3), Pointwise(MjNear(1e-7, 1e-3), Dh));
}
}
}
@@ -1405,7 +1407,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
mju_mulMatVec(res.data(), H.data(), vec.data(), nv, nv);
// finite difference of mj_passive for stiffness
double eps = 1e-6;
mjtNum eps = MjTol(1e-6, 1e-3);
mjData* data_perturbed = mj_copyData(NULL, model, data);
// apply perturbation
@@ -1425,7 +1427,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
// compare analytical result (H*vec) with FD result
for (int i = 0; i < nv; ++i) {
EXPECT_NEAR(res[i], fd_res[i], 5e-3)
EXPECT_THAT(res[i], MjNear(fd_res[i], 5e-3, 5.0))
<< "Stiffness Mismatch at DOF " << i;
}
@@ -1440,7 +1442,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
max_asymmetry = mju_max(max_asymmetry, diff);
}
}
EXPECT_LT(max_asymmetry, 1e-10)
EXPECT_THAT(max_asymmetry, MjNear(0, 1e-10, 5e-4))
<< "K matrix is not symmetric at angle " << angle;
// check positive semi-definiteness: v^T * K * v >= 0
@@ -1455,7 +1457,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
vKv += v[i] * K_full[i * nv + j] * v[j];
}
}
EXPECT_GE(vKv, -1e-8) << "K matrix is not PSD at angle " << angle;
EXPECT_GE(vKv, MjTol(-1e-8, -1e-5)) << "K matrix is not PSD at angle " << angle;
}
}
@@ -1475,7 +1477,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
// finite-difference derivatives
std::vector<mjtNum> qDerivFD(nD);
mju_zero(data->qDeriv, nD);
mjtNum eps = 1e-6;
mjtNum eps = MjTol(1e-6, 1e-3);
mjd_passive_velFD(model, data, eps);
mju_copy(qDerivFD.data(), data->qDeriv, nD);
@@ -1512,8 +1514,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
}
// expect FD and corrected analytic derivatives to match
mjtNum tol = 1e-4;
EXPECT_THAT(qDerivAnalytic, Pointwise(DoubleNear(tol), qDerivFD))
EXPECT_THAT(qDerivAnalytic, Pointwise(MjNear(1e-4, 1e4), qDerivFD))
<< "Damping Mismatch at angle: " << angle;
}
}
test/engine/engine_forward_test.cc
+47 -26
View File
@@ -44,6 +44,19 @@ namespace {
static const char* const kEnergyConservingPendulumPath =
"engine/testdata/derivative/energy_conserving_pendulum.xml";
// helper for precision-aware checks in macros (e.g. MJDATA_POINTERS)
template <typename T>
void ExpectNear(T a, T b) {
EXPECT_EQ(a, b);
}
template <>
void ExpectNear<mjtNum>(mjtNum a, mjtNum b) {
EXPECT_EQ(a, b);
}
static const char* const kDampedActuatorsPath =
"engine/testdata/derivative/damped_actuators.xml";
static const char* const kJointForceClamp =
@@ -52,7 +65,7 @@ static const char* const kTendonForceClamp =
"engine/testdata/actuation/tendon_force_clamp.xml";
using ::testing::Pointwise;
using ::testing::DoubleNear;
using ::testing::Ne;
using ::testing::HasSubstr;
using ::testing::NotNull;
@@ -100,7 +113,7 @@ TEST_P(ParametrizedForwardTest, ActLimited) {
EXPECT_GT(data->act[0], -1);
// after 99 steps we hit the upper bound
if (i < 99) EXPECT_LT(data->act[0], 1);
if (i >= 99) EXPECT_EQ(data->act[0], 1);
if (i >= 99) EXPECT_NEAR(data->act[0], 1, MjTol(0, 5e-6));
}
data->ctrl[0] = -1.0;
@@ -111,7 +124,9 @@ TEST_P(ParametrizedForwardTest, ActLimited) {
EXPECT_LT(data->act[0], model->actuator_actrange[1]);
// after 199 steps we hit the lower bound
if (i < 199) EXPECT_GT(data->act[0], model->actuator_actrange[0]);
if (i >= 199) EXPECT_EQ(data->act[0], model->actuator_actrange[0]);
if (i >= 199) {
EXPECT_NEAR(data->act[0], model->actuator_actrange[0], MjTol(0.0, 5e-6));
}
}
mj_deleteData(data);
@@ -256,7 +271,7 @@ TEST_F(ImplicitIntegratorTest, EulerDampDisable) {
qacc_fd[i] = (data->qvel[i] - qvel[i]) / model->opt.timestep;
}
// expect finite-differenced qacc to match to high precision
EXPECT_THAT(qacc_fd, Pointwise(DoubleNear(1e-14), qacc));
EXPECT_THAT(qacc_fd, Pointwise(MjNear(1e-14, 1e-6), qacc));
// reach the same initial state
mj_resetData(model, data);
@@ -372,11 +387,13 @@ TEST_F(ImplicitIntegratorTest, EulerImplicitEquivalent) {
}
// expect qpos vectors to be numerically different
#ifndef mjUSESINGLE
EXPECT_THAT(AsVector(data->qpos, model->nq), Pointwise(Ne(), qposEuler));
#endif
// expect qpos vectors to be similar to high precision
EXPECT_THAT(AsVector(data->qpos, model->nq),
Pointwise(DoubleNear(1e-14), qposEuler));
Pointwise(MjNear(1e-14, 1e-6), qposEuler));
mj_deleteData(data);
mj_deleteModel(model);
@@ -393,8 +410,10 @@ TEST_F(ImplicitIntegratorTest, JointActuatorEquivalent) {
mj_step(model, data);
}
// expect corresponding joint values to be significantly different
#ifndef mjUSESINGLE
EXPECT_GT(fabs(data->qpos[0]-data->qpos[2]), 1e-4);
EXPECT_GT(fabs(data->qpos[1]-data->qpos[3]), 1e-4);
#endif
// reset, take 10 steps with implicit
mj_resetData(model, data);
@@ -404,8 +423,8 @@ TEST_F(ImplicitIntegratorTest, JointActuatorEquivalent) {
}
// expect corresponding joint values to be insignificantly different
EXPECT_LT(fabs(data->qpos[0]-data->qpos[2]), 1e-16);
EXPECT_LT(fabs(data->qpos[1]-data->qpos[3]), 1e-16);
EXPECT_LT(fabs(data->qpos[0]-data->qpos[2]), MjTol(1e-16, 1e-6));
EXPECT_LT(fabs(data->qpos[1]-data->qpos[3]), MjTol(1e-16, 1e-6));
mj_deleteData(data);
mj_deleteModel(model);
@@ -671,6 +690,9 @@ TEST_F(ForwardTest, eq_active) {
// test that normalized and denormalized quats give the same result
TEST_F(ForwardTest, NormalizeQuats) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "Skipping in float32: exact mjData comparison infeasible.";
#endif
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicit">
@@ -729,7 +751,7 @@ TEST_F(ForwardTest, NormalizeQuats) {
#define X(type, name, nr, nc) \
for (int i = 0; i < model->nr; i++) \
for (int j = 0; j < nc; j++) \
EXPECT_EQ(data_n->name[i*nc+j], data_u->name[i*nc+j]);
ExpectNear(data_n->name[i*nc+j], data_u->name[i*nc+j]);
MJDATA_POINTERS;
#undef X
@@ -759,7 +781,7 @@ TEST_F(ForwardTest, NormalizeQuats) {
#define X(type, name, nr, nc) \
for (int i = 0; i < model->nr; i++) \
for (int j = 0; j < nc; j++) \
EXPECT_EQ(data_n->name[i*nc+j], data_u->name[i*nc+j]);
ExpectNear(data_n->name[i*nc+j], data_u->name[i*nc+j]);
MJDATA_POINTERS;
#undef X
@@ -791,8 +813,8 @@ TEST_F(ForwardTest, MocapQuats) {
// expect mocap_quat to be normalized (by the compiler)
for (int i = 0; i < 4; i++) {
EXPECT_EQ(data->mocap_quat[i], 0.5);
EXPECT_EQ(data->xquat[4+i], 0.5);
EXPECT_NEAR(data->mocap_quat[i], 0.5, MjTol(0, 1e-6));
EXPECT_NEAR(data->xquat[4+i], 0.5, MjTol(0, 1e-6));
}
// write denormalized quats to mocap_quat, call forward again
@@ -803,8 +825,8 @@ TEST_F(ForwardTest, MocapQuats) {
// expect mocap_quat to remain denormalized, but xquat to be normalized
for (int i = 0; i < 4; i++) {
EXPECT_EQ(data->mocap_quat[i], 1);
EXPECT_EQ(data->xquat[4+i], 0.5);
EXPECT_NEAR(data->mocap_quat[i], 1, MjTol(0, 1e-6));
EXPECT_NEAR(data->xquat[4+i], 0.5, MjTol(0, 1e-6));
}
mj_deleteData(data);
@@ -957,8 +979,8 @@ TEST_F(ActuatorTest, ActuatorForceClamping) {
mj_forward(model, data);
// expect clamping as specified in the model
EXPECT_EQ(data->actuator_force[0], 1);
EXPECT_EQ(data->qfrc_actuator[0], 0.4);
EXPECT_NEAR(data->actuator_force[0], 1, MjTol(0, 1e-6));
EXPECT_NEAR(data->qfrc_actuator[0], 0.4, MjTol(0, 1e-6));
// simulate for 2 seconds to gain velocity
while (data->time < 2) {
@@ -968,7 +990,7 @@ TEST_F(ActuatorTest, ActuatorForceClamping) {
// activate damper, expect force to be clamped at lower bound
data->ctrl[1] = 1;
mj_forward(model, data);
EXPECT_EQ(data->qfrc_actuator[0], -0.4);
EXPECT_NEAR(data->qfrc_actuator[0], -0.4, MjTol(0, 1e-6));
mj_deleteData(data);
mj_deleteModel(model);
@@ -1117,9 +1139,9 @@ TEST_F(ActuatorTest, DampRatioTendon) {
// expect first and second fingers to move together
double tol = 1e-10;
EXPECT_THAT(AsVector(data->qpos, 4),
Pointwise(DoubleNear(tol), AsVector(data->qpos + 4, 4)));
Pointwise(MjNear(tol, tol), AsVector(data->qpos + 4, 4)));
EXPECT_THAT(AsVector(data->qvel, 4),
Pointwise(DoubleNear(tol), AsVector(data->qvel + 4, 4)));
Pointwise(MjNear(tol, tol), AsVector(data->qvel + 4, 4)));
mj_deleteData(data);
mj_deleteModel(model);
@@ -1232,7 +1254,7 @@ TEST_F(FilterExactTest, TimestepIndependent) {
}
mjtNum large_timestep_act = data->act[0];
EXPECT_THAT(small_timestep_act, DoubleNear(large_timestep_act, 1e-14))
EXPECT_NEAR(small_timestep_act, large_timestep_act, MjTol(1e-14, 1e-6))
<< "exact integration should be independent of timestep to machine "
"precision.";
@@ -1313,8 +1335,7 @@ TEST_F(ActEarlyTest, RemovesOneStepDelay) {
mj_step(model, data);
for (int j = 0; j < model->nu / 2; j++) {
// this is true for torque actuators
EXPECT_THAT(last_qfrc[2 * j],
DoubleNear(data->qfrc_actuator[2 * j + 1], 1e-3))
EXPECT_NEAR(last_qfrc[2 * j], data->qfrc_actuator[2 * j + 1], MjTol(1e-3, 1e-1))
<< "there should be a 1 step delay between qfrc for "
<< mj_id2name(model, mjOBJ_ACTUATOR, 2 * j);
}
@@ -1549,7 +1570,7 @@ TEST_F(ForwardTest, ActuatorDelayLinearInterp) {
// delay = 0.015 seconds = 1.5*timestep, nsample=3, interp=1 (linear)
EXPECT_EQ(model->actuator_history[0], 3);
EXPECT_EQ(model->actuator_history[1], 1); // interp=1 (linear)
EXPECT_NEAR(model->actuator_delay[0], 0.015, 1e-10);
EXPECT_NEAR(model->actuator_delay[0], 0.015, MjTol(1e-10, 5e-6));
// Set increasing ctrl values
// Buffer has samples at times: -0.02, -0.01, 0 with values 0, 0, 0
@@ -1559,7 +1580,7 @@ TEST_F(ForwardTest, ActuatorDelayLinearInterp) {
data->ctrl[0] = 10.0;
mj_step(model, data);
EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10) << "step 0";
EXPECT_NEAR(data->actuator_force[0], 0.0, MjTol(1e-10, 5e-6)) << "step 0";
// After step 1 at time=0.02: buffer has times 0, 0.01, 0.02 with values 0, 10, 20
// Read at time 0.02 - 0.015 = 0.005: interpolate between t=0 (val=0) and t=0.01 (val=10)
@@ -1567,7 +1588,7 @@ TEST_F(ForwardTest, ActuatorDelayLinearInterp) {
data->ctrl[0] = 20.0;
mj_step(model, data);
EXPECT_NEAR(data->actuator_force[0], 5.0, 1e-10) << "step 1";
EXPECT_NEAR(data->actuator_force[0], 5.0, MjTol(1e-10, 5e-6)) << "step 1";
// After step 2 at time=0.03: buffer has times 0.01, 0.02, 0.03 with values 10, 20, 30
// Read at 0.03 - 0.015 = 0.015: interpolate between t=0.01 (val=10) and t=0.02 (val=20)
@@ -1575,7 +1596,7 @@ TEST_F(ForwardTest, ActuatorDelayLinearInterp) {
data->ctrl[0] = 30.0;
mj_step(model, data);
EXPECT_NEAR(data->actuator_force[0], 15.0, 1e-10) << "step 2";
EXPECT_NEAR(data->actuator_force[0], 15.0, MjTol(1e-10, 5e-6)) << "step 2";
mj_deleteData(data);
mj_deleteModel(model);
@@ -1774,7 +1795,7 @@ TEST_F(ForwardTest, FlexParentCoupling) {
if (diff > max_diff) max_diff = diff;
}
EXPECT_LT(max_diff, 2e-5)
EXPECT_LT(max_diff, MjTol(2e-5, 5e-3))
<< "Implicit integrator should match Euler at small timestep";
mj_deleteData(data);
test/engine/engine_inverse_test.cc
+2 -2
View File
@@ -51,7 +51,7 @@ TEST_F(InverseTest, ForwardInverseMatch) {
mj_compareFwdInv(model, data);
// expect mismatch to be small
mjtNum epsilon = 1e-10;
mjtNum epsilon = MjTol(1e-10, 0.05);
EXPECT_LT(data->solver_fwdinv[0], epsilon);
EXPECT_LT(data->solver_fwdinv[1], epsilon);
@@ -110,7 +110,7 @@ TEST_F(InverseTest, DiscreteInverseMatch) {
// depending on mjENBL_INVDISCRETE flag, expect mismatch to be small/large
if (invdiscrete) {
mjtNum epsilon = 1e-9;
mjtNum epsilon = MjTol(1e-9, 0.05);
EXPECT_LT(data->solver_fwdinv[0], epsilon);
EXPECT_LT(data->solver_fwdinv[1], epsilon);
} else {
test/engine/engine_passive_test.cc
+12 -11
View File
@@ -55,15 +55,15 @@ TEST_F(PassiveTest, DisableFlags) {
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qacc[0], 11);
EXPECT_MJTNUM_EQ(d->qacc[0], 11);
m->opt.disableflags = mjDSBL_DAMPER;
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qacc[0], 10);
EXPECT_MJTNUM_EQ(d->qacc[0], 10);
m->opt.disableflags = mjDSBL_SPRING;
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qacc[0], 1);
EXPECT_MJTNUM_EQ(d->qacc[0], 1);
m->opt.disableflags = mjDSBL_SPRING | mjDSBL_DAMPER;
mj_forward(m, d);
@@ -125,7 +125,7 @@ TEST_F(PassiveTest, PolyStiffnessSlide) {
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qfrc_spring[0], -48);
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
mj_deleteData(d);
mj_deleteModel(m);
@@ -154,7 +154,7 @@ TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) {
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qfrc_spring[0], 8);
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], 8);
mj_deleteData(d);
mj_deleteModel(m);
@@ -194,7 +194,7 @@ TEST_F(PassiveTest, PolyStiffnessTendon) {
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
EXPECT_FLOAT_EQ(d->qfrc_spring[0], -48);
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
mj_deleteData(d);
mj_deleteModel(m);
@@ -296,7 +296,8 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
d1->qpos[5] = 0.5;
d1->qpos[6] = 0.5;
const mjtNum tol = 1e-14; // tolerance for floating point numbers
// tolerance for floating point numbers
constexpr mjtNum tol = MjTol(1e-14, 1e-5);
EXPECT_EQ(m1->nv, m2->nv);
@@ -551,8 +552,8 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) {
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
}
}
EXPECT_NEAR(
4*energy/volume, 2*scale*scale, std::numeric_limits<float>::epsilon());
constexpr mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-5);
EXPECT_NEAR(4*energy/volume, 2*scale*scale, tol);
}
}
@@ -602,8 +603,8 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
}
}
EXPECT_NEAR(
energy/volume, 3*scale*scale, std::numeric_limits<float>::epsilon());
constexpr mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-4);
EXPECT_NEAR(energy/volume, 3*scale*scale, tol);
}
}
test/engine/engine_plugin_test.cc
+6 -6
View File
@@ -33,7 +33,6 @@
namespace mujoco {
namespace {
using ::testing::DoubleNear;
using ::testing::HasSubstr;
using ::testing::NotNull;
@@ -660,7 +659,8 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
m->plugin_stateadr[3]),
testing::ElementsAreArray<int>({4*(i+1), 8*j, 4*j}));
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[2]>(d->actuator_force),
testing::ElementsAreArray<mjtNum>({0.125*j, 0.25*j}));
testing::ElementsAreArray<mjtNum>(
{(mjtNum)0.125*j, (mjtNum)0.25*j}));
mj_step(m, d);
mj_forward(m, d);
}
@@ -702,19 +702,19 @@ TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
for (int i = 0; i < 6; ++i) {
// act_dot should be computed by the plugin
mjtNum expected_act_dot = TestActuator::kActDotValue;
EXPECT_THAT(d->act_dot[i], DoubleNear(expected_act_dot, 1e-6));
EXPECT_THAT(d->act_dot[i], MjNear(expected_act_dot, 1e-6, 1e-4));
// act_dot from the plugin should be Euler-integrated
mjtNum expected_act = expected_act_dot * m->opt.timestep;
EXPECT_THAT(d->act[i], DoubleNear(expected_act, 1e-6));
EXPECT_THAT(d->act[i], MjNear(expected_act, 1e-6, 1e-4));
}
// actuator filter state should be updated outside the plugin for filter
// actuators.
mjtNum expected_act_dot = 0.5 / m->actuator_dynprm[mjNDYN * 2];
EXPECT_THAT(d->act_dot[6], DoubleNear(expected_act_dot, 1e-6));
EXPECT_THAT(d->act_dot[6], MjNear(expected_act_dot, 1e-6, 1e-4));
EXPECT_THAT(d->act[6],
DoubleNear(0.5 + expected_act_dot * m->opt.timestep, 1e-6));
MjNear(0.5 + expected_act_dot * m->opt.timestep, 1e-6, 1e-4));
mj_deleteData(d);
mj_deleteModel(m);
test/engine/engine_ray_test.cc
+27 -23
View File
@@ -78,7 +78,6 @@ static constexpr char kCubeletModel[] = R"(
using ::std::string;
using ::testing::AnyOf;
using ::testing::DoubleNear;
using ::testing::ElementsAre;
using ::testing::NotNull;
using ::testing::Pointwise;
@@ -102,7 +101,7 @@ TEST_F(RayTest, NoExclusions) {
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
EXPECT_FLOAT_EQ(distance, 0.9);
EXPECT_MJTNUM_EQ(distance, 0.9);
mj_deleteData(data);
mj_deleteModel(model);
}
@@ -125,26 +124,26 @@ TEST_F(RayTest, Exclusions) {
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
EXPECT_FLOAT_EQ(distance, 0.9);
EXPECT_NEAR(distance, 0.9, MjTol(1e-12, 1e-5));
// Exclude nearest geom
geomgroup[1] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
EXPECT_FLOAT_EQ(distance, 2.9);
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
geomgroup[0] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2");
EXPECT_FLOAT_EQ(distance, 4.9);
EXPECT_NEAR(distance, 4.9, MjTol(1e-12, 1e-5));
geomgroup[2] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid, nullptr);
EXPECT_EQ(geomid, -1);
EXPECT_FLOAT_EQ(distance, -1);
EXPECT_NEAR(distance, -1, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
@@ -168,7 +167,7 @@ TEST_F(RayTest, ExcludeStatic) {
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
EXPECT_FLOAT_EQ(distance, 2.9);
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
@@ -216,7 +215,7 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
for (int j = 0; j < M; ++j) {
int idx = i * M + j;
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, nullptr);
EXPECT_FLOAT_EQ(dist, dist_multiray[idx]);
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
nhits += dist >= 0;
}
@@ -271,11 +270,11 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
int idx = i * M + j;
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
normal);
EXPECT_FLOAT_EQ(dist, dist_multiray[idx]);
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
EXPECT_FLOAT_EQ(normal[0], normal_multiray[3*idx]);
EXPECT_FLOAT_EQ(normal[1], normal_multiray[3*idx + 1]);
EXPECT_FLOAT_EQ(normal[2], normal_multiray[3*idx + 2]);
EXPECT_MJTNUM_EQ(normal[0], normal_multiray[3*idx]);
EXPECT_MJTNUM_EQ(normal[1], normal_multiray[3*idx + 1]);
EXPECT_MJTNUM_EQ(normal[2], normal_multiray[3*idx + 2]);
nhits += dist >= 0;
}
}
@@ -303,10 +302,10 @@ TEST_F(RayTest, EdgeCases) {
// pnt contained in bounding box
mjtNum pnt1[] = {-1, 0, 0};
mju_multiRayPrepare(m, d, pnt1, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
EXPECT_FLOAT_EQ(geom_ba[0], -mjPI);
EXPECT_FLOAT_EQ(geom_ba[1], 0);
EXPECT_FLOAT_EQ(geom_ba[2], mjPI);
EXPECT_FLOAT_EQ(geom_ba[3], mjPI);
EXPECT_MJTNUM_EQ(geom_ba[0], -mjPI);
EXPECT_MJTNUM_EQ(geom_ba[1], 0);
EXPECT_MJTNUM_EQ(geom_ba[2], mjPI);
EXPECT_MJTNUM_EQ(geom_ba[3], mjPI);
mjtNum vec1[] = {1, 0, 0};
mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
@@ -472,6 +471,9 @@ void _rayMeshTest(const mjModel* m) {
}
TEST_F(RayTest, RayMeshPruning) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "BVH pruning incorrectly rejects intersections in float32";
#endif
char error[1024];
const string xml_path =
GetTestDataFilePath("engine/testdata/ray/stanford_bunny.xml");
@@ -529,11 +531,10 @@ TEST_F(RayTest, RayHfield) {
mj_forward(model, data);
double tol = 1e-8;
EXPECT_THAT(data->sensordata[0], DoubleNear(1, tol));
EXPECT_THAT(data->sensordata[1], DoubleNear(1, tol));
EXPECT_THAT(data->sensordata[2], DoubleNear(1, tol));
EXPECT_THAT(data->sensordata[3], DoubleNear(0.5, tol));
EXPECT_THAT(data->sensordata[0], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[1], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[2], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[3], MjNear(0.5, 1e-8, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
@@ -551,6 +552,9 @@ static const char* const kHfieldModel = "engine/testdata/ray/hfield.xml";
static const char* const kFlexModel = "engine/testdata/ray/flex.xml";
TEST_F(RayTest, RayNormal) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "Flex face normals differ significantly in float32";
#endif
for (const char* path : {kPlaneModel, kSphereModel, kCapsuleModel,
kEllipsoidModel, kCylinderModel, kBoxModel,
kMeshModel, kSdfModel, kHfieldModel, kFlexModel}) {
@@ -646,8 +650,8 @@ TEST_F(RayTest, RayNormal) {
mjtNum expected_neg[3] = {-expected[0], -expected[1], -expected[2]};
// compare analytic with fin-diff approximation
EXPECT_THAT(normal, AnyOf(Pointwise(DoubleNear(100 * eps), expected),
Pointwise(DoubleNear(100 * eps), expected_neg)))
EXPECT_THAT(normal, AnyOf(Pointwise(MjNear(100*eps, 1e-3), expected),
Pointwise(MjNear(100*eps, 1e-3), expected_neg)))
<< path << ", time " << d->time;
// increment count
test/engine/engine_sensor_test.cc
+57 -56
View File
@@ -35,7 +35,7 @@ namespace {
using ::std::string;
using ::std::vector;
using ::testing::DoubleNear;
using ::testing::ElementsAre;
using ::testing::ElementsAreArray;
using ::testing::HasSubstr;
@@ -139,13 +139,13 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMat) {
// compare actual and expected values
vector pos = GetSensor(model, data, 0);
EXPECT_THAT(pos, Pointwise(DoubleNear(tol), {5, 5, 0}));
EXPECT_THAT(pos, Pointwise(MjNear(tol, 1e-6), {5, 5, 0}));
vector xaxis = GetSensor(model, data, 1);
EXPECT_THAT(xaxis, Pointwise(DoubleNear(tol), {0, -1, 0}));
EXPECT_THAT(xaxis, Pointwise(MjNear(tol, 1e-6), {0, -1, 0}));
vector yaxis = GetSensor(model, data, 2);
EXPECT_THAT(yaxis, Pointwise(DoubleNear(tol), {1, 0, 0}));
EXPECT_THAT(yaxis, Pointwise(MjNear(tol, 1e-6), {1, 0, 0}));
mj_deleteData(data);
mj_deleteModel(model);
@@ -182,7 +182,7 @@ TEST_F(RelativeFrameSensorTest, ReferenceQuatMat) {
// compare quaternion sensor and quat derived from orientation matrix
vector quat = GetSensor(model, data, 3);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), converted_quat));
EXPECT_THAT(quat, Pointwise(MjNear(tol, 1e-6), converted_quat));
mj_deleteData(data);
mj_deleteModel(model);
@@ -238,7 +238,7 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) {
data->sensordata+nsensordata);
// object and reference have moved together, we expect values to not change
EXPECT_THAT(actual_values, Pointwise(DoubleNear(tol), expected_values));
EXPECT_THAT(actual_values, Pointwise(MjNear(tol, 1e-6), expected_values));
mj_deleteData(data);
mj_deleteModel(model);
@@ -273,7 +273,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelLinearFixed) {
// compare to expected values
vector linvel = GetSensor(model, data, 0);
const mjtNum expected_linvel[3] = {-mju_sqrt(0.5), mju_sqrt(0.5), 0};
EXPECT_THAT(linvel, Pointwise(DoubleNear(tol), expected_linvel));
EXPECT_THAT(linvel, Pointwise(MjNear(tol, 1e-6), expected_linvel));
mj_deleteData(data);
mj_deleteModel(model);
@@ -305,7 +305,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngFixed) {
// obj and ref rotate together, relative angular velocities should be zero
vector angvel = GetSensor(model, data, 0);
EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), {0, 0, 0}));
EXPECT_THAT(angvel, Pointwise(MjNear(tol, 1e-6), {0, 0, 0}));
mj_deleteData(data);
mj_deleteModel(model);
@@ -342,7 +342,7 @@ TEST_F(RelativeFrameSensorTest, FrameVelAngOpposing) {
// obj and ref rotate on same axis, we can just difference the velocities
vector angvel = GetSensor(model, data, 0);
const mjtNum expected_angvel[3] = {0, data->qvel[1]-data->qvel[0], 0};
EXPECT_THAT(angvel, Pointwise(DoubleNear(tol), expected_angvel));
EXPECT_THAT(angvel, Pointwise(MjNear(tol, 1e-6), expected_angvel));
mj_deleteData(data);
mj_deleteModel(model);
@@ -413,9 +413,10 @@ TEST_F(RelativeFrameSensorTest, FrameVelGeneral) {
mju_quat2Vel(angvel_findiff, dquat, dt);
// compare analytic and finite-differenced relative velocities
EXPECT_THAT(linvel, Pointwise(DoubleNear(10*dt), linvel_findiff));
EXPECT_THAT(angvel, Pointwise(DoubleNear(10*dt), angvel_findiff));
EXPECT_THAT(linvel, Pointwise(MjNear(10 * dt, 0.15),
linvel_findiff));
EXPECT_THAT(angvel,
Pointwise(MjNear(10 * dt, 0.2), angvel_findiff));
mj_deleteData(data);
mj_deleteModel(model);
@@ -442,11 +443,11 @@ TEST_F(SensorTest, EnableEnergy) {
mjData* data = mj_makeData(model);
mj_forward(model, data);
EXPECT_EQ(data->energy[0], 2*3*5);
EXPECT_NEAR(data->energy[0], 2*3*5, MjTol(1e-12, 1e-5));
model->opt.enableflags &= ~mjENBL_ENERGY;
mj_forward(model, data);
EXPECT_EQ(data->energy[0], 0);
EXPECT_NEAR(data->energy[0], 0, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
@@ -471,11 +472,11 @@ TEST_F(SensorTest, PotentialEnergy) {
mjData* data = mj_makeData(model);
mj_forward(model, data);
EXPECT_EQ(data->sensordata[0], 2*3*5);
EXPECT_NEAR(data->sensordata[0], 2*3*5, MjTol(1e-12, 1e-5));
data->qpos[2] = 7;
mj_forward(model, data);
EXPECT_EQ(data->sensordata[0], 7*3*5);
EXPECT_NEAR(data->sensordata[0], 7*3*5, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
@@ -502,7 +503,7 @@ TEST_F(SensorTest, PotentialEnergyFreeJointSpring) {
data->qpos[1] = 2;
data->qpos[2] = 3;
mj_forward(model, data);
EXPECT_EQ(data->sensordata[0], 0.5*2*14);
EXPECT_NEAR(data->sensordata[0], 0.5*2*14, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
@@ -532,7 +533,8 @@ TEST_F(SensorTest, KineticEnergy) {
mjtNum mass = 3;
mjtNum speed = data->time * mju_norm3(model->opt.gravity);
EXPECT_FLOAT_EQ(data->sensordata[0], 0.5 * mass * speed * speed);
EXPECT_NEAR(data->sensordata[0], 0.5 * mass * speed * speed,
MjTol(1e-7, 1e-2));
mj_deleteData(data);
mj_deleteModel(model);
@@ -569,7 +571,7 @@ TEST_F(SensorTest, PolyStiffnessEnergy) {
for (int i = 0; i < 100; i++) {
mj_step(m, d);
EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002);
EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.003);
}
mj_deleteData(d);
@@ -674,40 +676,39 @@ TEST_F(SensorTest, CollisionSequential) {
mjData* data = mj_makeData(model);
mj_forward(model, data);
EXPECT_DOUBLE_EQ(data->sensordata[0], 0.8);
EXPECT_DOUBLE_EQ(data->sensordata[1], 0.7);
EXPECT_DOUBLE_EQ(data->sensordata[2], 0.5);
mjtNum eps = 1e-14;
EXPECT_NEAR(data->sensordata[0], 0.8, MjTol(eps, 1e-7));
EXPECT_NEAR(data->sensordata[1], 0.7, MjTol(eps, 1e-7));
EXPECT_NEAR(data->sensordata[2], 0.5, MjTol(eps, 1e-7));
EXPECT_THAT(GetSensor(model, data, 3),
Pointwise(DoubleNear(eps), vector<mjtNum>{0, 0, 1}));
Pointwise(MjNear(eps, 1e-7), vector<mjtNum>{0, 0, 1}));
EXPECT_THAT(GetSensor(model, data, 4),
Pointwise(DoubleNear(eps), vector<mjtNum>{0, 0, -1}));
Pointwise(MjNear(eps, 1e-7), vector<mjtNum>{0, 0, -1}));
EXPECT_THAT(GetSensor(model, data, 5),
Pointwise(DoubleNear(eps), vector<mjtNum>{1, 0, 0}));
Pointwise(MjNear(eps, 1e-7), vector<mjtNum>{1, 0, 0}));
EXPECT_THAT(GetSensor(model, data, 6),
Pointwise(DoubleNear(eps),
Pointwise(MjNear(eps, 1e-6),
vector<mjtNum>{0, 0, 0, 0, 0, .8}));
EXPECT_THAT(GetSensor(model, data, 7),
Pointwise(DoubleNear(eps),
Pointwise(MjNear(eps, 1e-6),
vector<mjtNum>{1, 0, .7, 1, 0, 0}));
EXPECT_THAT(GetSensor(model, data, 8),
Pointwise(DoubleNear(eps),
Pointwise(MjNear(eps, 1e-6),
vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
EXPECT_THAT(GetSensor(model, data, 9),
Pointwise(DoubleNear(eps), GetSensor(model, data, 0)));
Pointwise(MjNear(eps, 1e-7), GetSensor(model, data, 0)));
EXPECT_THAT(GetSensor(model, data, 10),
Pointwise(DoubleNear(eps), GetSensor(model, data, 6)));
Pointwise(MjNear(eps, 1e-6), GetSensor(model, data, 6)));
EXPECT_THAT(GetSensor(model, data, 11),
Pointwise(DoubleNear(eps), GetSensor(model, data, 3)));
Pointwise(MjNear(eps, 1e-7), GetSensor(model, data, 3)));
EXPECT_THAT(GetSensor(model, data, 12),
Pointwise(DoubleNear(eps), GetSensor(model, data, 5)));
Pointwise(MjNear(eps, 1e-7), GetSensor(model, data, 5)));
EXPECT_THAT(GetSensor(model, data, 13),
Pointwise(DoubleNear(eps), GetSensor(model, data, 8)));
Pointwise(MjNear(eps, 1e-6), GetSensor(model, data, 8)));
EXPECT_THAT(GetSensor(model, data, 14),
Pointwise(DoubleNear(eps), GetSensor(model, data, 2)));
Pointwise(MjNear(eps, 1e-7), GetSensor(model, data, 2)));
mj_deleteData(data);
mj_deleteModel(model);
@@ -804,23 +805,23 @@ TEST_F(SensorTest, Contact) {
vector sitewall = GetSensor(model, data, "site:wall");
EXPECT_EQ(sitewall, vector<mjtNum>{1});
mjtNum tol = 1e-4;
vector wall = GetSensor(model, data, "wall");
EXPECT_THAT(wall, Pointwise(DoubleNear(tol), {1, 8, 0, 0, -1, 0, 0,
0, 0, 0, 0, 0, 0, 0}));
EXPECT_THAT(wall, Pointwise(MjNear(1e-4, 0.02),
{1, 8, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0}));
// normals points *away* from b2 (towards floor / b1)
vector b2 = GetSensor(model, data, "b2");
EXPECT_THAT(b2, Pointwise(DoubleNear(tol), {3, 0, 0, 0, 0, -1,
4, 0, 0, 1, 0, 0}));
EXPECT_THAT(b2, Pointwise(MjNear(1e-4, 0.02),
{3, 0, 0, 0, 0, -1, 4, 0, 0, 1, 0, 0}));
// normal points *towards* b2
vector b2f = GetSensor(model, data, "b2_flipped");
EXPECT_THAT(b2f, Pointwise(DoubleNear(tol), {3, 0, 0, 0, 0, 1,
4, 0, 0, -1, 0, 0}));
EXPECT_THAT(b2f, Pointwise(MjNear(1e-4, 0.02),
{3, 0, 0, 0, 0, 1, 4, 0, 0, -1, 0, 0}));
vector b2r = GetSensor(model, data, "b2_reduced");
EXPECT_THAT(b2r, Pointwise(DoubleNear(tol), {4, 0, 0, -1, 0, 0}));
EXPECT_THAT(b2r,
Pointwise(MjNear(1e-4, 0.02), {4, 0, 0, -1, 0, 0}));
}
mj_deleteData(data);
@@ -972,7 +973,7 @@ TEST_F(SensorTest, ContactNet) {
mj_applyFT(model, data, force, torque, point, b1, qfrc.data());
// compare
EXPECT_THAT(qfrc, Pointwise(DoubleNear(1e-6), qfrc_expected));
EXPECT_THAT(qfrc, Pointwise(MjNear(1e-6, 1e-4), qfrc_expected));
// check net force, sensor returns body2 -> body1
vector net21 = GetSensor(model, data, "net21");
@@ -991,7 +992,7 @@ TEST_F(SensorTest, ContactNet) {
mj_applyFT(model, data, force, torque, point, b2, qfrc.data());
// compare
EXPECT_THAT(qfrc, Pointwise(DoubleNear(1e-6), qfrc_expected));
EXPECT_THAT(qfrc, Pointwise(MjNear(1e-6, 1e-4), qfrc_expected));
nconmax = std::max(nconmax, data->ncon);
}
@@ -1257,7 +1258,7 @@ TEST_F(SensorTest, SensorDelay) {
// delay = 0.02 seconds, timestep = 0.01
// history = 3 (more than delay/timestep=2) to ensure buffer coverage
EXPECT_EQ(model->sensor_history[0], 3);
EXPECT_NEAR(model->sensor_delay[0], 0.02, 1e-10);
EXPECT_NEAR(model->sensor_delay[0], 0.02, MjTol(1e-10, 1e-7));
// Use different values to verify exact delay timing.
// With delay=0.02 and timestep=0.01, we expect 2-step delay:
@@ -1319,7 +1320,7 @@ TEST_F(SensorTest, SensorDelayLinearInterp) {
// exactly between two buffer samples, so we should get the average.
EXPECT_EQ(model->sensor_history[0], 3);
EXPECT_EQ(model->sensor_history[1], 1); // interp=1 (linear)
EXPECT_NEAR(model->sensor_delay[0], 0.015, 1e-10);
EXPECT_NEAR(model->sensor_delay[0], 0.015, MjTol(1e-10, 1e-7));
// Set increasing qpos values: step i -> qpos = (i+1)*10
// Buffer has samples at times: -0.02, -0.01, 0 (initialized)
@@ -1421,9 +1422,9 @@ TEST_F(SensorTest, SensorInterval) {
if (triggers0[t]) value0 = t;
if (triggers1[t]) value1 = t;
EXPECT_NEAR(data->sensordata[adr0], value0, 1e-10)
EXPECT_NEAR(data->sensordata[adr0], value0, MjTol(1e-10, 1e-7))
<< "sensor0 at t=" << t;
EXPECT_NEAR(data->sensordata[adr1], value1, 1e-10)
EXPECT_NEAR(data->sensordata[adr1], value1, MjTol(1e-10, 1e-7))
<< "sensor1 at t=" << t;
}
@@ -1453,8 +1454,8 @@ TEST_F(SensorTest, SensorDelayInterval) {
// Combined delay and interval
EXPECT_EQ(model->sensor_history[0], 5);
EXPECT_NEAR(model->sensor_delay[0], 0.02, 1e-10);
EXPECT_NEAR(model->sensor_interval[2*0], 0.03, 1e-10);
EXPECT_NEAR(model->sensor_delay[0], 0.02, MjTol(1e-10, 1e-7));
EXPECT_NEAR(model->sensor_interval[2*0], 0.03, MjTol(1e-10, 1e-7));
// Verify initial buffer timestamps (after mj_makeData/mj_resetData)
// With period=0.03, dt=0.01, nsample=5, phase=0 (means -period=-0.03):
@@ -1465,7 +1466,7 @@ TEST_F(SensorTest, SensorDelayInterval) {
mjtNum* times = buf + 2;
mjtNum expected_times[] = {-0.15, -0.12, -0.09, -0.06, -0.03};
for (int i = 0; i < n; i++) {
EXPECT_NEAR(times[i], expected_times[i], 1e-10);
EXPECT_NEAR(times[i], expected_times[i], MjTol(1e-10, 0.015));
}
// set position
@@ -1478,12 +1479,12 @@ TEST_F(SensorTest, SensorDelayInterval) {
for (int i = 0; i < 2; i++) {
mj_step(model, data);
// sensor reads delayed value (0.0 from initial buffer)
EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step " << i;
EXPECT_NEAR(data->sensordata[0], 0.0, MjTol(1e-10, 1e-7)) << "step " << i;
}
// step 3 (i=2): reading at t=0.00 now returns the inserted value 5.0
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 5.0, 1e-10);
EXPECT_NEAR(data->sensordata[0], 5.0, MjTol(1e-10, 1e-7));
mj_deleteData(data);
mj_deleteModel(model);
@@ -1511,8 +1512,8 @@ TEST_F(SensorTest, SensorHistoryOnly) {
// history only, no delay or interval
EXPECT_EQ(model->sensor_history[0], 5);
EXPECT_NEAR(model->sensor_delay[0], 0.0, 1e-10);
EXPECT_NEAR(model->sensor_interval[0], 0.0, 1e-10);
EXPECT_NEAR(model->sensor_delay[0], 0.0, MjTol(1e-10, 1e-7));
EXPECT_NEAR(model->sensor_interval[2*0], 0.0, MjTol(1e-10, 1e-7));
// set position
data->qpos[0] = 3.0;
test/engine/engine_setconst_test.cc
+7 -8
View File
@@ -27,7 +27,6 @@ namespace mujoco {
namespace {
using ::std::string;
using ::testing::DoubleNear;
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::NotNull;
@@ -457,20 +456,20 @@ TEST_F(SetConstTest, DofLength) {
EXPECT_EQ(model->dof_length[0], 1);
// B2: Hinge
EXPECT_THAT(model->dof_length[1], DoubleNear(3, tol));
EXPECT_NEAR(model->dof_length[1], 3, tol);
// B3: Ball
EXPECT_THAT(model->dof_length[2], DoubleNear(4, tol));
EXPECT_THAT(model->dof_length[3], DoubleNear(4, tol));
EXPECT_THAT(model->dof_length[4], DoubleNear(4, tol));
EXPECT_NEAR(model->dof_length[2], 4, tol);
EXPECT_NEAR(model->dof_length[3], 4, tol);
EXPECT_NEAR(model->dof_length[4], 4, tol);
// B4: Free
EXPECT_EQ(model->dof_length[5], 1);
EXPECT_EQ(model->dof_length[6], 1);
EXPECT_EQ(model->dof_length[7], 1);
EXPECT_THAT(model->dof_length[8], DoubleNear(5, tol));
EXPECT_THAT(model->dof_length[9], DoubleNear(5, tol));
EXPECT_THAT(model->dof_length[10], DoubleNear(5, tol));
EXPECT_NEAR(model->dof_length[8], 5, tol);
EXPECT_NEAR(model->dof_length[9], 5, tol);
EXPECT_NEAR(model->dof_length[10], 5, tol);
mj_deleteModel(model);
}
test/engine/engine_solver_test.cc
+8 -7
View File
@@ -26,7 +26,6 @@
namespace mujoco {
namespace {
using ::testing::DoubleNear;
using ::testing::NotNull;
using ::testing::Pointwise;
using ::std::max;
@@ -93,10 +92,11 @@ TEST_F(SolverTest, IslandsEquivalent) {
auto time = std::to_string(data_noisland->time);
for (int j = 0; j < nv; j++) {
// increase tolerance for large elements
mjtNum scale = 0.5 * max(2.0, abs(data_noisland->qacc[j]) +
abs(data_island->qacc[j]));
EXPECT_THAT(data_noisland->qacc[j],
DoubleNear(data_island->qacc[j], scale * rtol[i]))
mjtNum scale = 0.5 * max(static_cast<mjtNum>(2.0),
std::abs(data_noisland->qacc[j]) +
std::abs(data_island->qacc[j]));
EXPECT_NEAR(data_noisland->qacc[j], data_island->qacc[j],
MjTol(scale * rtol[i], 500 * scale * rtol[i]))
<< "time: " << time << '\n'
<< "dof: " << j << '\n'
<< "maxiter: " << maxiter[i] << '\n'
@@ -159,7 +159,7 @@ TEST_F(SolverTest, IslandsEquivalentForward) {
mju_norm(data_island->qacc, nv));
mjtNum tol = scale * (solver == mjSOL_CG ? 1e-6 : 1e-8);
EXPECT_THAT(AsVector(data_island->qacc, nv),
Pointwise(DoubleNear(scale * tol),
Pointwise(MjNear(scale * tol, 500 * scale * tol),
AsVector(data_noisland->qacc, nv)))
<< "warmstart: " << warmstart << '\n'
<< "jacobian: " << (jacobian ? "sparse" : "dense") << '\n'
@@ -269,7 +269,8 @@ TEST_F(SolverTest, SolversEquivalent) {
(jacobian == mjJAC_DENSE ? "dense" : "sparse");
EXPECT_THAT(AsVector(data->qfrc_constraint, nv),
Pointwise(DoubleNear(tolerance),
Pointwise(MjNear(tolerance,
max(1e-1, 1000 * tolerance)),
AsVector(data_truth->qfrc_constraint, nv)))
<< "model: " << config.path << "\n"
<< "cone: " << cone_str << "\n"
test/engine/engine_support_test.cc
+32 -31
View File
@@ -34,7 +34,6 @@ namespace {
using ::std::vector;
using ::testing::ContainsRegex; // NOLINT
using ::testing::DoubleNear;
using ::testing::Eq;
using ::testing::MatchesRegex;
using ::testing::Ne;
@@ -99,9 +98,8 @@ TEST_F(AngMomMatTest, CompareAngMom) {
mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv);
// compare the two angular momentum values
static const mjtNum tol = 1e-8;
for (int i = 0; i < 3; i++) {
EXPECT_THAT(angmom_ref[i], DoubleNear(angmom_test[i], tol));
EXPECT_THAT(angmom_ref[i], MjNear(angmom_test[i], 1e-8, 1e-4));
}
mju_free(angmom_mat);
@@ -129,7 +127,7 @@ TEST_F(AngMomMatTest, CompareAngMomMats) {
mj_angmomMat(model, data, angmom_mat, bodyid);
// compute the angular momentum matrix using finite differences
static const mjtNum eps = 1e-6;
static constexpr mjtNum eps = MjTol(1e-6, 1e-3);
for (int i = 0; i < nv; i++) {
// reset vel, forward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv);
@@ -154,9 +152,8 @@ TEST_F(AngMomMatTest, CompareAngMomMats) {
}
// compare the two matrices
static const mjtNum tol = 1e-8;
for (int i = 0; i < 3*nv; i++) {
EXPECT_THAT(angmom_mat_fd[i], DoubleNear(angmom_mat[i], tol));
EXPECT_THAT(angmom_mat_fd[i], MjNear(angmom_mat[i], 1e-8, 2e-4));
}
mju_free(angmom_mat_fd);
@@ -241,7 +238,7 @@ TEST_F(JacobianTest, SubtreeJac) {
// compare finite-differenced and analytic Jacobian
for (int j=0; j < 3; j++) {
mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps;
EXPECT_THAT(jac_subtree[nv*j+i], DoubleNear(findiff, eps));
EXPECT_THAT(jac_subtree[nv*j+i], MjNear(findiff, eps, 1e-2));
}
}
@@ -289,7 +286,7 @@ TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
for (int r = 0; r < 3; r++) {
for (int c = 0; c < nv; c++) {
mjtNum expected = c - body_dofadr == r ? invtreemass : 0.0;
EXPECT_THAT(jac_subtree[nv*r+c], DoubleNear(expected, max_abs_err));
EXPECT_THAT(jac_subtree[nv*r+c], MjNear(expected, max_abs_err, 1e-4));
}
}
@@ -459,7 +456,7 @@ TEST_F(JacobianTest, JacDot) {
mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid);
// jac_h: jacobian after integrating qpos with a timestep of h
mjtNum h = 1e-7;
constexpr mjtNum h = MjTol(1e-7, 5e-4);
mj_integratePos(model, data->qpos, data->qvel, h);
mj_kinematics(model, data);
mj_comPos(model, data);
@@ -478,8 +475,8 @@ TEST_F(JacobianTest, JacDot) {
// compare finite-differenced and analytic
mjtNum tol = 1e-5;
EXPECT_THAT(jacp_dot, Pointwise(DoubleNear(tol), jacp_dot_h));
EXPECT_THAT(jacr_dot, Pointwise(DoubleNear(tol), jacr_dot_h));
EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h));
EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h));
mj_deleteData(data);
mj_deleteModel(model);
@@ -683,7 +680,7 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
mju_quat2Vel(qvel_expect, qdif, dt);
// expect numerical equality
EXPECT_THAT(AsVector(qvel, 3), Pointwise(DoubleNear(eps), qvel_expect));
EXPECT_THAT(AsVector(qvel, 3), Pointwise(MjNear(eps, 1e-3), qvel_expect));
}
}
@@ -1007,7 +1004,7 @@ TEST_F(InertiaTest, mulM) {
mj_mulM(model, data, res2.data(), vec.data());
// expect vectors to match to floating point precision
EXPECT_THAT(res1, Pointwise(DoubleNear(1e-10), res2));
EXPECT_THAT(res1, Pointwise(MjNear(1e-10, 0.1), res2));
mj_deleteData(data);
mj_deleteModel(model);
@@ -1038,7 +1035,7 @@ TEST_F(InertiaTest, mulM2) {
// 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_FLOAT_EQ(sqrtMvec2, vecMvec);
EXPECT_MJTNUM_EQ(sqrtMvec2, vecMvec);
mj_deleteData(data);
mj_deleteModel(model);
@@ -1093,7 +1090,7 @@ TEST_F(InertiaTest, FullM) {
mju_mulMatTMat(P.data(), L.data(), DL.data(), nv, nv, nv);
// expect M and P to match to high precision
EXPECT_THAT(M, Pointwise(DoubleNear(1e-10), P));
EXPECT_THAT(M, Pointwise(MjNear(1e-10, 1e-4), P));
mj_deleteData(d);
mj_deleteModel(m);
@@ -1144,39 +1141,43 @@ TEST_F(SupportTest, GeomDistance) {
// plane-sphere
distmax = 1.0;
EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 0, 1, 1.0, fromto), 0.8);
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(DoubleNear(eps),
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5),
vector<mjtNum>{0, 0, 0, 0, 0, 0.8}));
// sphere-plane
EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 1, 0, 1.0, fromto), 0.8);
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
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<mjtNum>{0, 0, 0.8, 0, 0, 0}));
// sphere-sphere
EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 1, 2, 1.0, fromto), 0.5);
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
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<mjtNum>{.2, 0, 1, .7, 0, 1}));
// sphere-sphere, flipped order
EXPECT_DOUBLE_EQ(mj_geomDistance(model, data, 2, 1, 1.0, fromto), 0.5);
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
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<mjtNum>{.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),
DoubleNear(0.7, eps));
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
MjNear(0.7, eps, eps*100));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps*100),
vector<mjtNum>{0, 0, .1, 0, 0, .8}));
// mesh-sphere (far distmax)
distmax = 1.0;
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
DoubleNear(0.7, eps));
EXPECT_THAT(fromto, Pointwise(DoubleNear(eps),
MjNear(0.7, eps, eps*100));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps*100),
vector<mjtNum>{0, 0, .1, 0, 0, .8}));
mj_deleteData(data);
@@ -1203,7 +1204,7 @@ TEST_F(SupportTest, GeomDistanceFromToFlipped) {
fromto10[0], fromto10[1], fromto10[2]};
EXPECT_THAT(AsVector(fromto10flipped, 6),
Pointwise(DoubleNear(1.0e-12), fromto01));
Pointwise(MjNear(1.0e-12, 1e-5), fromto01));
}
mj_deleteData(data);
mj_deleteModel(model);
@@ -1318,7 +1319,7 @@ TEST_F(SupportTest, ReadCtrlWithDelay) {
// 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-10);
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)
@@ -1456,11 +1457,11 @@ TEST_F(SupportTest, InitSensorDelay) {
mjtNum result = 0;
const mjtNum* ptr = mj_readSensor(model, data, 0, 0.04, &result, /*order=*/0);
mjtNum val = ptr ? *ptr : result;
EXPECT_EQ(val, 0.7);
EXPECT_NEAR(val, 0.7, 1e-6);
ptr = mj_readSensor(model, data, 0, 0.03, &result, /*order=*/0);
val = ptr ? *ptr : result;
EXPECT_EQ(val, 0.6);
EXPECT_NEAR(val, 0.6, 1e-6);
mj_deleteData(data);
mj_deleteModel(model);
test/engine/engine_util_blas_test.cc
+4 -4
View File
@@ -155,10 +155,10 @@ TEST_F(EngineUtilBlasTest, Normalize4IsIdempotent) {
mju_normalize4(quat_renormalized);
// expect equality
EXPECT_EQ(quat[0], quat_renormalized[0]);
EXPECT_EQ(quat[1], quat_renormalized[1]);
EXPECT_EQ(quat[2], quat_renormalized[2]);
EXPECT_EQ(quat[3], quat_renormalized[3]);
EXPECT_MJTNUM_EQ(quat[0], quat_renormalized[0]);
EXPECT_MJTNUM_EQ(quat[1], quat_renormalized[1]);
EXPECT_MJTNUM_EQ(quat[2], quat_renormalized[2]);
EXPECT_MJTNUM_EQ(quat[3], quat_renormalized[3]);
}
}
test/engine/engine_util_misc_test.cc
+22 -22
View File
@@ -32,7 +32,6 @@
namespace mujoco {
namespace {
using ::testing::DoubleNear;
using ::testing::ElementsAre;
using ::testing::ElementsAreArray;
using ::testing::HasSubstr;
@@ -68,21 +67,22 @@ TEST_F(UtilMiscTest, Sigmoid) {
EXPECT_EQ(mju_sigmoid(2), 1);
// epsilon for finite-differencing
const mjtNum dx = 1e-7;
constexpr mjtNum dx = MjTol(1e-7, 1e-3);
constexpr mjtNum fd_tol = MjTol(1e-7, 1e-3);
// derivative at 0
mjtNum dy_dx_0 = (mju_sigmoid(0 + dx) - mju_sigmoid(0)) / dx;
EXPECT_THAT(dy_dx_0, DoubleNear(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_THAT(dy_dx_1, DoubleNear(0, 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_THAT(dy_dx_0p5, DoubleNear(expected, dx));
EXPECT_NEAR(dy_dx_0p5, expected, fd_tol);
}
TEST_F(UtilMiscTest, SphereWrap) {
@@ -431,9 +431,9 @@ TEST_F(InterpolationTest, mju_interpolate3D) {
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);
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));
}
}
@@ -484,9 +484,9 @@ TEST_F(InterpolationTest, mju_defGradient) {
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}));
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(DoubleNear(1e-8), {0, -1, 0, 1, 0, 0, 0, 0, 1}));
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];
@@ -499,9 +499,9 @@ TEST_F(InterpolationTest, mju_defGradient) {
mju_quat2Mat(rot6, quat);
}
mju_defGradient(mat, p1, dof6, order);
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot6));
EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), rot6));
mju_defGradient(mat, p2, dof6, order);
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot6));
EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), rot6));
// z-axis CoM rotation
mjtNum dof7[24];
@@ -516,9 +516,9 @@ TEST_F(InterpolationTest, mju_defGradient) {
mju_quat2Mat(rot7, quat);
}
mju_defGradient(mat, p1, dof7, order);
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot7));
EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), rot7));
mju_defGradient(mat, p2, dof7, order);
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot7));
EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), rot7));
}
// --------------------------------- Base64 ------------------------------------
@@ -895,8 +895,8 @@ TEST_F(HistoryTest, ReadVector_Linear) {
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
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) {
@@ -1027,24 +1027,24 @@ TEST_F(HistoryTest, CubicInterpolation) {
// 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);
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, 1e-9);
EXPECT_NEAR(res[1], 1.0 - expected_0_25, 1e-9);
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, 1e-9);
EXPECT_NEAR(res[1], 1.0 - expected_0_8, 1e-9);
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));
}
} // namespace
test/engine/engine_util_solve_test.cc
+31 -24
View File
@@ -34,9 +34,9 @@
namespace mujoco {
namespace {
using ::testing::DoubleEq;
using ::testing::Pointwise;
using ::testing::DoubleNear;
using ::testing::ElementsAre;
using ::std::string;
using ::std::vector;
@@ -92,7 +92,8 @@ mjtNum objective(const mjtNum* x, const mjtNum* H, const mjtNum* g, int n) {
// utility: test if res is the minimum of a given box-QP problem
bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n,
const mjtNum* lower, const mjtNum* upper) {
static const mjtNum eps = 1e-4; // epsilon used for nudging
constexpr mjtNum eps = MjTol(1e-4, 5e-2); // epsilon used for nudging
constexpr mjtNum threshold = MjTol(0, -2e-3); // comparison threshold
bool is_minimum = true;
mjtNum* res_nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*n);
@@ -109,7 +110,7 @@ bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n,
res_nudge[i] = mju_max(lower[i], res_nudge[i]);
}
value_nudge = objective(res_nudge, H, g, n);
if (value_nudge - value < 0) {
if (value_nudge - value < threshold) {
is_minimum = false;
break;
}
@@ -120,7 +121,7 @@ bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n,
res_nudge[i] = mju_min(upper[i], res_nudge[i]);
}
value_nudge = objective(res_nudge, H, g, n);
if (value_nudge - value < 0) {
if (value_nudge - value < threshold) {
is_minimum = false;
break;
}
@@ -185,14 +186,14 @@ TEST_F(BoxQPTest, UnboundedQP) {
// no bounds, expect Newton point
EXPECT_EQ(nfree, 2);
EXPECT_THAT(res[0], DoubleEq(-g[0]/H[0]));
EXPECT_THAT(res[1], DoubleEq(-g[1]/H[3]));
EXPECT_MJTNUM_EQ(res[0], -g[0]/H[0]);
EXPECT_MJTNUM_EQ(res[1], -g[1]/H[3]);
// check that solution is actual minimum
EXPECT_TRUE(isQPminimum(res, H, g, n, /*lower=*/nullptr, /*upper=*/nullptr));
// perturb solution, expected it no longer be the minimum
res[0] += 0.001;
res[0] += MjTol(0.001, 0.1);
EXPECT_FALSE(isQPminimum(res, H, g, n, /*lower=*/nullptr, /*upper=*/nullptr));
// negative-definite Hessian, no solution
@@ -223,8 +224,8 @@ TEST_F(BoxQPTest, AsymmetricUpperIgnored) {
EXPECT_EQ(nfree, 1);
EXPECT_THAT(res[0], DoubleEq(-g[0]/H[0]));
EXPECT_THAT(res[1], DoubleEq(lower[1]));
EXPECT_MJTNUM_EQ(res[0], -g[0]/H[0]);
EXPECT_MJTNUM_EQ(res[1], lower[1]);
}
// test mju_boxQP on a single random bounded QP
@@ -244,9 +245,9 @@ TEST_F(BoxQPTest, BoundedQP) {
// use default options
int maxiter = 100; // maximum number of iterations
mjtNum mingrad = 1E-16; // minimum squared norm of (unclamped) gradient
mjtNum mingrad = MjTol(1E-16, 1E-5); // minimum squared norm of (unclamped) gradient
mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize
mjtNum minstep = 1E-22; // minimum stepsize for linesearch
mjtNum minstep = MjTol(1E-22, 1E-10); // minimum stepsize for linesearch
mjtNum armijo = 0.1; // Armijo parameter
// logging
@@ -285,6 +286,12 @@ TEST_F(BoxQPTest, BoundedQP) {
// test mju_boxQP on a set of random bounded QPs
TEST_F(BoxQPTest, BoundedQPvariations) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP()
<< "BoxQP test permutations contain ill-conditioned matrices which "
"natively fail to find descent directions under float32 precision";
}
int nmax = 100;
// allocate maximum size on heap
@@ -317,9 +324,9 @@ TEST_F(BoxQPTest, BoundedQPvariations) {
// default algorithm options
int maxiter = 100;
mjtNum mingrad = 1E-16;
mjtNum mingrad = MjTol(1E-16, 1E-5);
mjtNum backtrack = 0.5;
mjtNum minstep = 1E-22;
mjtNum minstep = MjTol(1E-22, 1E-10);
mjtNum armijo = 0.1;
// solve box-QP with logging
@@ -500,9 +507,9 @@ TEST_F(BandMatrixTest, Multiplication) {
/*nVec=*/1, /*flg_sym=*/1);
// expect numerical equality
mjtNum eps = 1e-12;
mjtNum eps = MjTol(1e-12, 5e-5);
EXPECT_THAT(AsVector(res, nTotal),
Pointwise(DoubleNear(eps), AsVector(res1, nTotal)));
Pointwise(MjNear(eps, eps), AsVector(res1, nTotal)));
mju_free(res1);
mju_free(res);
@@ -565,9 +572,9 @@ TEST_F(BandMatrixTest, Factorization) {
}
// expect numerical equality
mjtNum eps = 1e-12;
mjtNum eps = MjTol(1e-12, 5e-5);
EXPECT_THAT(AsVector(H, nH),
Pointwise(DoubleNear(eps), AsVector(H1, nH)));
Pointwise(MjNear(eps, eps), AsVector(H1, nH)));
// multiply dense
mju_mulMatVec(res, H, vec, nTotal, nTotal);
@@ -578,7 +585,7 @@ TEST_F(BandMatrixTest, Factorization) {
// expect numerical equality
EXPECT_THAT(AsVector(res, nTotal),
Pointwise(DoubleNear(eps), AsVector(res1, nTotal)));
Pointwise(MjNear(eps, eps), AsVector(res1, nTotal)));
mju_free(res1);
mju_free(res);
@@ -638,9 +645,9 @@ TEST_F(BandMatrixTest, Solve) {
mju_cholSolveBand(res1, B, vec, nTotal, nBand, nDense);
// expect numerical equality
mjtNum eps = 1e-12;
mjtNum eps = MjTol(1e-12, 5e-5);
EXPECT_THAT(AsVector(res, nTotal),
Pointwise(DoubleNear(eps), AsVector(res1, nTotal)));
Pointwise(MjNear(eps, eps), AsVector(res1, nTotal)));
mju_free(res1);
mju_free(res);
@@ -808,7 +815,7 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdate) {
n, n);
// compare
mjtNum eps = 1e-8;
mjtNum eps = MjTol(1e-8, 5e-4);
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
EXPECT_NEAR(H_reconstructed[i * n + j], H_expected[i * n + j], eps)
@@ -922,7 +929,7 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) {
L_sparse_dense.data(), n, n, n);
// compare
mjtNum eps = 1e-8;
mjtNum eps = MjTol(1e-8, 5e-4);
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
EXPECT_NEAR(H_sparse_reconstructed[i * n + j], H_expected[i * n + j],
@@ -1025,7 +1032,7 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
EXPECT_EQ(rank_new, n);
// compare L values
mjtNum eps = 1e-10;
mjtNum eps = MjTol(1e-10, 5e-5);
for (int i = 0; i < nnz; i++) {
EXPECT_NEAR(L_new[i], L_ref[i], eps) << "mismatch at index " << i;
}
test/engine/engine_util_spatial_test.cc
+16 -16
View File
@@ -30,7 +30,6 @@
namespace mujoco {
namespace {
using ::testing::DoubleNear;
using ::testing::ElementsAre;
using ::testing::Pointwise;
@@ -122,7 +121,7 @@ TEST_F(RotVecQuatTest, TestEquivalence) {
{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {-0.5, 1, -0.5}, {1.22, -2.33, 3.44}};
// List of angles to rotate by, in degrees
mjtNum angles[6] = {0.0, 1e-8, 31, 47, 181, 271};
static const mjtNum eps = 1e-15;
static constexpr mjtNum eps = MjTol(1e-15, 1e-5);
for (auto vec : vecs) {
// Unit-normalize the vector
mju_normalize3(vec);
@@ -163,38 +162,38 @@ TEST_F(Euler2QuatTest, BadSeqLength) {
TEST_F(Euler2QuatTest, Euler2Quat) {
mjtNum quat[4] = {0};
mjtNum tol = 1e-14;
mjtNum tol = MjTol(1e-14, 1e-6);
char seq[] = "xyz";
mjtNum euler[3] = {mjPI, 0, 0};
mjtNum expected[4] = {0, 1, 0, 0};
mju_euler2Quat(quat, euler, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected));
EXPECT_THAT(quat, Pointwise(MjNear(tol, tol), expected));
euler[1] = mjPI;
mjtNum expected2[4] = {0, 0, 0, 1};
mju_euler2Quat(quat, euler, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected2));
EXPECT_THAT(quat, Pointwise(MjNear(tol, tol), expected2));
char seq2[] = "XYZ";
mjtNum expected3[4] = {0, 0, 0, -1};
mju_euler2Quat(quat, euler, seq2);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected3));
EXPECT_THAT(quat, Pointwise(MjNear(tol, tol), expected3));
mjtNum euler2[3] = {2*mjPI, 2*mjPI, 2*mjPI};
mjtNum expected4[4] = {-1, 0, 0, 0};
mju_euler2Quat(quat, euler2, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4));
EXPECT_THAT(quat, Pointwise(MjNear(tol, tol), expected4));
mju_euler2Quat(quat, euler2, seq2);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4));
EXPECT_THAT(quat, Pointwise(MjNear(tol, tol), expected4));
mjtNum euler3[3] = {mjPI/2, mjPI/2, mjPI/2};
mjtNum expected5[4] = {0, mju_sqrt(.5), 0, mju_sqrt(.5)};
mju_euler2Quat(quat, euler3, seq);
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected5));
EXPECT_THAT(quat, Pointwise(MjNear(tol, tol), expected5));
mju_euler2Quat(quat, euler3, seq2);
mjtNum expected6[4] = {mju_sqrt(.5), 0, mju_sqrt(.5), 0};
EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected6));
EXPECT_THAT(quat, Pointwise(MjNear(tol, tol), expected6));
}
using Mat2RotTest = MujocoTest;
@@ -219,8 +218,9 @@ TEST_F(Mat2RotTest, RotationFromArbitraryMatrix) {
// calculate rotational part of the matrix
mjtNum quat[4] = {1, 0, 0, 0};
int niter = mju_mat2Rot(quat, mat);
EXPECT_THAT(quat, Pointwise(DoubleNear(1e-8), target));
EXPECT_LE(niter, 150);
EXPECT_THAT(quat, Pointwise(MjNear(1e-8, 1e-6), target));
int max_iter = static_cast<int>(MjTol(150, 500));
EXPECT_LE(niter, max_iter);
}
TEST_F(Mat2RotTest, IdentityFromRandomRotation) {
@@ -244,7 +244,7 @@ TEST_F(Mat2RotTest, IdentityFromRandomRotation) {
mju_normalize4(quat);
EXPECT_LE(mju_mat2Rot(quat, mat), 40);
mju_quat2Mat(res, quat);
EXPECT_THAT(res, Pointwise(DoubleNear(1e-6), mat));
EXPECT_THAT(res, Pointwise(MjNear(1e-6, 1e-6), mat));
}
}
@@ -252,13 +252,13 @@ TEST_F(Mat2RotTest, SpecialCases) {
mjtNum eye[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
mjtNum quat[4] = {1, 0, 0, 0};
EXPECT_EQ(mju_mat2Rot(quat, eye), 0);
EXPECT_THAT(quat, Pointwise(DoubleNear(1e-8), {1, 0, 0, 0}));
EXPECT_THAT(quat, Pointwise(MjNear(1e-8, 1e-8), {1, 0, 0, 0}));
mjtNum zero[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
EXPECT_EQ(mju_mat2Rot(quat, zero), 0);
EXPECT_THAT(quat, Pointwise(DoubleNear(1e-8), {1, 0, 0, 0}));
EXPECT_THAT(quat, Pointwise(MjNear(1e-8, 1e-4), {1, 0, 0, 0}));
mjtNum ones[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1};
EXPECT_EQ(mju_mat2Rot(quat, ones), 0);
EXPECT_THAT(quat, Pointwise(DoubleNear(1e-8), {1, 0, 0, 0}));
EXPECT_THAT(quat, Pointwise(MjNear(1e-8, 1e-4), {1, 0, 0, 0}));
}
} // namespace
test/fixture.h
+43
View File
@@ -26,6 +26,7 @@
#include <string_view>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <absl/container/flat_hash_map.h>
#include <absl/container/flat_hash_set.h>
@@ -39,6 +40,48 @@ MJAPI decltype(mju_user_error) _mjPRIVATE__get_tls_error_fn();
namespace mujoco {
// Precision-aware GMock matcher. Use instead of DoubleNear/FloatNear.
// Under double builds, uses double_tol. Under float builds, uses float_tol.
template <typename T1, typename T2>
inline auto MjNear(T1 double_tol, T2 float_tol) {
#ifdef mjUSESINGLE
return ::testing::FloatNear(static_cast<float>(float_tol));
#else
return ::testing::DoubleNear(static_cast<double>(double_tol));
#endif
}
// Precision-aware GMock matcher (3-arg version).
// Under double builds, matches near target with double_tol.
// Under float builds, matches near target with float_tol.
template <typename T1, typename T2, typename T3>
inline auto MjNear(T1 target, T2 double_tol, T3 float_tol) {
#ifdef mjUSESINGLE
return ::testing::FloatNear(static_cast<float>(target),
static_cast<float>(float_tol));
#else
return ::testing::DoubleNear(static_cast<double>(target),
static_cast<double>(double_tol));
#endif
}
// Precision-aware tolerance for EXPECT_NEAR.
template <typename T1, typename T2>
constexpr mjtNum MjTol(T1 double_tol, T2 float_tol) {
#ifdef mjUSESINGLE
return static_cast<mjtNum>(float_tol);
#else
return static_cast<mjtNum>(double_tol);
#endif
}
// Precision-aware equality assertion: 4 ULPs in either precision.
#ifdef mjUSESINGLE
#define EXPECT_MJTNUM_EQ(a, b) EXPECT_FLOAT_EQ(a, b)
#else
#define EXPECT_MJTNUM_EQ(a, b) EXPECT_DOUBLE_EQ(a, b)
#endif
// Installs and uninstalls error callbacks on MuJoCo that fail the currently
// running test if triggered. Prefer the use of MujocoTest, unless using a
// test fixture is not possible.
test/pipeline_test.cc
+11 -5
View File
@@ -30,7 +30,6 @@ namespace {
static const char* const kDefaultModel = "testdata/model.xml";
using ::testing::Pointwise;
using ::testing::DoubleNear;
using ::testing::NotNull;
using PipelineTest = MujocoTest;
@@ -43,12 +42,19 @@ TEST_F(PipelineTest, SparseDenseEquivalent) {
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
mjtNum tol = 1e-11;
constexpr mjtNum tol = MjTol(1e-11, 1e-4);
const char* sname[4] = {"NEWTON", "PGS", "CG", "NOSLIP"};
mjtSolver solver[4] = {mjSOL_NEWTON, mjSOL_PGS, mjSOL_CG, mjSOL_NEWTON};
for (int i : {0, 1, 2, 3}) {
#ifdef mjUSESINGLE
// CG and NOSLIP sparse-dense equivalence breaks at float32 precision.
int nsolvers = 2;
#else
int nsolvers = 4;
#endif
for (int i = 0; i < nsolvers; i++) {
model->opt.solver = solver[i];
if (i == 3) {
model->opt.noslip_iterations = 2;
@@ -69,10 +75,10 @@ TEST_F(PipelineTest, SparseDenseEquivalent) {
std::vector<mjtNum> qpos_sparse = AsVector(data->qpos, model->nq);
// expect accelerations to be insignificantly different
EXPECT_THAT(qacc_dense, Pointwise(DoubleNear(tol), qacc_sparse))
EXPECT_THAT(qacc_dense, Pointwise(MjNear(tol, tol), qacc_sparse))
<< "failed qacc equivalence for solver=" << sname[i];
// expect positions to be insignificantly different
EXPECT_THAT(qpos_dense, Pointwise(DoubleNear(tol), qpos_sparse))
EXPECT_THAT(qpos_dense, Pointwise(MjNear(tol, tol), qpos_sparse))
<< "failed qpos equivalence for solver=" << sname[i];
}
test/plugin/actuator/pid_test.cc
+6 -6
View File
@@ -30,7 +30,7 @@ namespace mujoco {
namespace {
using PidTest = MujocoTest;
using ::testing::DoubleNear;
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::NotNull;
@@ -395,9 +395,9 @@ TEST_F(PidTest, ITerm) {
mj_step(m, d);
}
EXPECT_THAT(d->qpos[0], DoubleNear(1.0 - 10 / 40.0, 1e-5));
EXPECT_THAT(d->qpos[1], DoubleNear(1.0, 1e-5));
EXPECT_THAT(d->qpos[2], DoubleNear(1.0 - (10 - 0.125 * 40.0) / 40.0, 1e-5));
EXPECT_THAT(d->qpos[0], MjNear(1.0 - 10 / 40.0, 1e-5, 1e-5));
EXPECT_THAT(d->qpos[1], MjNear(1.0, 1e-5, 1e-5));
EXPECT_THAT(d->qpos[2], MjNear(1.0 - (10 - 0.125 * 40.0) / 40.0, 1e-5, 1e-5));
}
TEST_F(PidTest, FiniteDifferencing) {
@@ -494,14 +494,14 @@ TEST_F(PidTest, CtrlClamp) {
for (int i = 0; i < 10000; i++) {
mj_step(m, d);
}
EXPECT_THAT(d->qpos[0], DoubleNear(0.75, 1e-5));
EXPECT_THAT(d->qpos[0], MjNear(0.75, 1e-5, 1e-5));
// when applying 0, it should be clamped to 0.25
d->ctrl[0] = 0.0;
for (int i = 0; i < 10000; i++) {
mj_step(m, d);
}
EXPECT_THAT(d->qpos[0], DoubleNear(0.25, 1e-5));
EXPECT_THAT(d->qpos[0], MjNear(0.25, 1e-5, 1e-5));
}
TEST_F(PidTest, CopyData) {
test/plugin/elasticity/elasticity_test.cc
+3
View File
@@ -31,6 +31,9 @@ using ElasticityTest = MujocoTest;
// -------------------------------- cable -----------------------------------
TEST_F(ElasticityTest, CantileverIntoCircle) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "Cable simulation with extreme stiffness diverges in float32";
#endif
static constexpr char cantilever_xml[] = R"(
<mujoco>
<option gravity="0 0 0"/>
test/user/user_flex_test.cc
+7 -2
View File
@@ -29,7 +29,6 @@
namespace mujoco {
namespace {
using ::testing::DoubleNear;
using ::testing::IsNull;
using ::testing::NotNull;
using ::testing::HasSubstr;
@@ -266,6 +265,9 @@ TEST_F(UserFlexTest, FlexNotCollide) {
}
TEST_F(UserFlexTest, BoundingBoxCoordinates) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "Float32 rounding in bounding box centering gives ~3e-8 error";
#endif
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
@@ -428,6 +430,9 @@ TEST_F(UserFlexTest, TrilinearInterpolation) {
}
TEST_F(UserFlexTest, StiffnessMatrix) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "Stiffness matrix kernel check fails in float32 precision";
#endif
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
@@ -452,7 +457,7 @@ TEST_F(UserFlexTest, StiffnessMatrix) {
ones[i] = 1;
}
mju_mulMatVec(res, m->flex_stiffness, ones, 3*m->nflexnode, 3*m->nflexnode);
EXPECT_THAT(res, Pointwise(DoubleNear(1e-8), zeros));
EXPECT_THAT(res, Pointwise(MjNear(1e-8, 1e-4), zeros));
mj_deleteModel(m);
}
test/user/user_mesh_test.cc
+37 -37
View File
@@ -979,14 +979,14 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
mju_mulPose(recovered_pos, recovered_quat,
&model->geom_pos[0], &model->geom_quat[0],
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 0, 1e-12);
EXPECT_NEAR(recovered_pos[1], 0, 1e-12);
EXPECT_NEAR(recovered_pos[2], 0, 1e-12);
EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[0], 1, 1e-12);
EXPECT_NEAR(recovered_quat[1], 0, 1e-12);
EXPECT_NEAR(recovered_quat[2], 0, 1e-12);
EXPECT_NEAR(recovered_quat[3], 0, 1e-12);
EXPECT_NEAR(recovered_quat[0], 1, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[1], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[2], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6));
// same test on the other geom
mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
@@ -994,14 +994,14 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
mju_mulPose(recovered_pos, recovered_quat,
&model->geom_pos[3], &model->geom_quat[4],
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 1, 1e-12);
EXPECT_NEAR(recovered_pos[1], 2, 1e-12);
EXPECT_NEAR(recovered_pos[2], 3, 1e-12);
EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[0], 0.5, 1e-12);
EXPECT_NEAR(recovered_quat[1], 0.5, 1e-12);
EXPECT_NEAR(recovered_quat[2], 0.5, 1e-12);
EXPECT_NEAR(recovered_quat[3], 0.5, 1e-12);
EXPECT_NEAR(recovered_quat[0], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6));
mj_deleteModel(model);
}
@@ -1038,14 +1038,14 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
mju_mulPose(recovered_pos, recovered_quat,
&model->geom_pos[0], &model->geom_quat[0],
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 0, 1e-12);
EXPECT_NEAR(recovered_pos[1], 0, 1e-12);
EXPECT_NEAR(recovered_pos[2], 0, 1e-12);
EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[0], 1, 1e-12);
EXPECT_NEAR(recovered_quat[1], 0, 1e-12);
EXPECT_NEAR(recovered_quat[2], 0, 1e-12);
EXPECT_NEAR(recovered_quat[3], 0, 1e-12);
EXPECT_NEAR(recovered_quat[0], 1, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[1], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[2], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6));
// same test on the other geom
mju_negPose(inverse_mesh_pos, inverse_mesh_quat,
@@ -1053,14 +1053,14 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
mju_mulPose(recovered_pos, recovered_quat,
&model->geom_pos[3], &model->geom_quat[4],
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 1, 1e-12);
EXPECT_NEAR(recovered_pos[1], 2, 1e-12);
EXPECT_NEAR(recovered_pos[2], 3, 1e-12);
EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[0], 0.5, 1e-12);
EXPECT_NEAR(recovered_quat[1], 0.5, 1e-12);
EXPECT_NEAR(recovered_quat[2], 0.5, 1e-12);
EXPECT_NEAR(recovered_quat[3], 0.5, 1e-12);
EXPECT_NEAR(recovered_quat[0], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6));
mj_deleteModel(model);
}
@@ -1443,7 +1443,7 @@ TEST_F(MjCMeshTest, OctreeHangingNodeInterpolation) {
mjModel* model = mj_compile(spec, 0);
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_GT(model->mesh_octnum[0], 0);
double kEps = 1e-6;
const mjtNum kEps = MjTol(1e-6, 1e-4);
const int octree_adr = model->mesh_octadr[0];
const int noct = model->mesh_octnum[0];
@@ -1604,14 +1604,14 @@ TEST_F(MjCMeshTest, OctreeNotComputedForNonSDF) {
mj_deleteModel(model);
}
double CubeSDF(double p[3], double b[3]) {
double q[3] = {std::abs(p[0]) - b[0],
std::abs(p[1]) - b[1],
std::abs(p[2]) - b[2]};
return std::sqrt(std::pow(std::max(q[0], 0.0), 2) +
std::pow(std::max(q[1], 0.0), 2) +
std::pow(std::max(q[2], 0.0), 2)) +
std::min(std::max(q[0], std::max(q[1], q[2])), 0.0);
mjtNum CubeSDF(mjtNum p[3], mjtNum b[3]) {
mjtNum q[3] = {mju_abs(p[0]) - b[0],
mju_abs(p[1]) - b[1],
mju_abs(p[2]) - b[2]};
return mju_sqrt(std::pow(std::max(q[0], (mjtNum)0), 2) +
std::pow(std::max(q[1], (mjtNum)0), 2) +
std::pow(std::max(q[2], (mjtNum)0), 2)) +
std::min(std::max(q[0], std::max(q[1], q[2])), (mjtNum)0);
}
TEST_F(MjCMeshTest, OctreeCube) {
test/user/user_model_test.cc
+5 -7
View File
@@ -31,7 +31,6 @@
namespace mujoco {
namespace {
using ::testing::DoubleNear;
using ::testing::ElementsAre;
using ::testing::HasSubstr;
using ::testing::IsNull;
@@ -168,8 +167,8 @@ TEST_F(UserModelTest, SameFrame) {
// expect them to be equal
constexpr double eps = 1e-6;
EXPECT_THAT(geom_xpos, Pointwise(DoubleNear(eps), geom_xpos2));
EXPECT_THAT(geom_xmat, Pointwise(DoubleNear(eps), geom_xmat2));
EXPECT_THAT(geom_xpos, Pointwise(MjNear(eps, eps), geom_xpos2));
EXPECT_THAT(geom_xmat, Pointwise(MjNear(eps, eps), geom_xmat2));
mj_deleteData(data);
mj_deleteModel(model);
@@ -606,7 +605,7 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) {
mj_step(m_fuse, d_fuse);
mj_step(m_no_fuse, d_no_fuse);
EXPECT_THAT(d_fuse->qvel[0], DoubleNear(d_no_fuse->qvel[0], 2e-17))
EXPECT_NEAR(d_fuse->qvel[0], d_no_fuse->qvel[0], MjTol(2e-17, 1e-8))
<< "Velocity should be the same after 1 step";
EXPECT_NE(d_fuse->qvel[0], 0);
@@ -887,9 +886,8 @@ TEST_F(LengthRangeTest, LengthRangeThreading) {
EXPECT_THAT(model1, NotNull()) << error;
// model is such that the lengthrange for first actuator is [1, sqrt(5)]
EXPECT_THAT(model1->actuator_lengthrange[0], DoubleNear(1.0, 1e-3));
EXPECT_THAT(model1->actuator_lengthrange[1],
DoubleNear(std::sqrt(5.0), 1e-3));
EXPECT_NEAR(model1->actuator_lengthrange[0], 1.0, 1e-3);
EXPECT_NEAR(model1->actuator_lengthrange[1], std::sqrt(5.0), 1e-3);
// recompile without threads
ASSERT_EQ(spec->compiler.usethread, 1);
test/user/user_objects_test.cc
+46 -32
View File
@@ -31,10 +31,9 @@
namespace mujoco {
namespace {
constexpr double kInertiaTol = 1e-6;
constexpr double kInertiaTol = MjTol(1e-6, 1e-6);
using std::string;
using ::testing::DoubleNear;
using ::testing::ElementsAre;
using ::testing::HasSubstr;
using ::testing::IsNull;
@@ -385,10 +384,10 @@ TEST_F(KeyframeTest, CheckValues) {
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
ASSERT_THAT(model, NotNull());
EXPECT_EQ(model->nkey, 7);
EXPECT_EQ(model->key_time[0 * 1], 0.1);
EXPECT_EQ(model->key_qpos[1 * model->nq], 0.2);
EXPECT_EQ(model->key_qvel[2 * model->nv], 0.3);
EXPECT_EQ(model->key_act[3 * model->na], 0.4);
EXPECT_MJTNUM_EQ(model->key_time[0 * 1], 0.1);
EXPECT_MJTNUM_EQ(model->key_qpos[1 * model->nq], 0.2);
EXPECT_MJTNUM_EQ(model->key_qvel[2 * model->nv], 0.3);
EXPECT_MJTNUM_EQ(model->key_act[3 * model->na], 0.4);
EXPECT_THAT(AsVector(model->key_ctrl + 4 * model->nu, model->nu),
ElementsAre(0.5, 0.6));
EXPECT_THAT(AsVector(model->key_mpos + 3 * model->nmocap * 5, 3),
@@ -405,20 +404,20 @@ TEST_F(KeyframeTest, ResetDataKeyframe) {
mjData* data = mj_makeData(model);
mj_resetDataKeyframe(model, data, 0);
EXPECT_EQ(data->time, 0.1);
EXPECT_MJTNUM_EQ(data->time, 0.1);
mj_resetDataKeyframe(model, data, 1);
EXPECT_EQ(data->qpos[0], 0.2);
EXPECT_MJTNUM_EQ(data->qpos[0], 0.2);
mj_resetDataKeyframe(model, data, 2);
EXPECT_EQ(data->qvel[0], 0.3);
EXPECT_MJTNUM_EQ(data->qvel[0], 0.3);
mj_resetDataKeyframe(model, data, 3);
EXPECT_EQ(data->act[0], 0.4);
EXPECT_MJTNUM_EQ(data->act[0], 0.4);
mj_resetDataKeyframe(model, data, 4);
EXPECT_EQ(data->ctrl[0], 0.5);
EXPECT_EQ(data->ctrl[1], 0.6);
EXPECT_MJTNUM_EQ(data->ctrl[0], 0.5);
EXPECT_MJTNUM_EQ(data->ctrl[1], 0.6);
mj_resetDataKeyframe(model, data, 5);
EXPECT_THAT(AsVector(data->mocap_pos, 3), ElementsAre(.1, .2, .3));
@@ -750,6 +749,9 @@ TEST_F(MjCGeomTest, CapsuleInertiaX) {
}
TEST_F(MjCGeomTest, ShellInertiaSphere) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision";
}
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
@@ -793,7 +795,7 @@ TEST_F(MjCGeomTest, ShellInertiaSphere) {
mjtNum mass3 = m->body_mass[3];
mjtNum mass4 = m->body_mass[4];
EXPECT_FLOAT_EQ(mass4 - mass3, m->body_mass[2]);
EXPECT_MJTNUM_EQ(mass4 - mass3, m->body_mass[2]);
// compute approximate shell inertia by subtracting inertias of massive bodies
// with small radius difference
@@ -809,6 +811,9 @@ TEST_F(MjCGeomTest, ShellInertiaSphere) {
}
TEST_F(MjCGeomTest, ShellInertiaCapsule) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision";
}
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
@@ -890,6 +895,9 @@ TEST_F(MjCGeomTest, ShellInertiaCapsule) {
}
TEST_F(MjCGeomTest, ShellInertiaCylinder) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision";
}
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
@@ -965,6 +973,9 @@ TEST_F(MjCGeomTest, ShellInertiaCylinder) {
}
TEST_F(MjCGeomTest, ShellInertiaEllipsoid) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision";
}
// test special case of ellipsoid with dimensions: a = b = c
// TODO(taylorhowell): add test for ellipsoid with dimensions: a != b != c
static constexpr char xml[] = R"(
@@ -1028,6 +1039,9 @@ TEST_F(MjCGeomTest, ShellInertiaEllipsoid) {
}
TEST_F(MjCGeomTest, ShellInertiaBox) {
if constexpr (sizeof(mjtNum) == sizeof(float)) {
GTEST_SKIP() << "ShellInertia tests use radii differences of ~1e-8, which vanish in float32 precision";
}
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
@@ -1247,16 +1261,16 @@ TEST_F(MjCJointTest, AlignFree) {
mj_forward(m_u, d_u);
// expect x-frames (sensors) to match to very high precision
double eps = 1e-10;
double eps = MjTol(1e-10, 1e-6);
EXPECT_THAT(
AsVector(d->sensordata, m->nsensordata),
Pointwise(DoubleNear(eps), AsVector(d_u->sensordata, m->nsensordata)));
Pointwise(MjNear(eps, eps), AsVector(d_u->sensordata, m->nsensordata)));
// no frame sensors for lights, test separately
EXPECT_THAT(AsVector(d->light_xpos, 3),
Pointwise(DoubleNear(eps), AsVector(d_u->light_xpos, 3)));
Pointwise(MjNear(eps, eps), AsVector(d_u->light_xpos, 3)));
EXPECT_THAT(AsVector(d->light_xdir, 3),
Pointwise(DoubleNear(eps), AsVector(d_u->light_xdir, 3)));
Pointwise(MjNear(eps, eps), AsVector(d_u->light_xdir, 3)));
// reduce timestep to 0.1ms and use RK4, simulate for 1 second
m->opt.timestep = m_u->opt.timestep = 1e-4;
@@ -1275,14 +1289,14 @@ TEST_F(MjCJointTest, AlignFree) {
mj_freeStack(d);
// expect x-frames to match to reasonable precision
eps = 1e-5;
eps = MjTol(1e-5, 1e-2);
EXPECT_THAT(
AsVector(d->sensordata, m->nsensordata),
Pointwise(DoubleNear(eps), AsVector(d_u->sensordata, m->nsensordata)));
Pointwise(MjNear(eps, eps), AsVector(d_u->sensordata, m->nsensordata)));
EXPECT_THAT(AsVector(d->light_xpos, 3),
Pointwise(DoubleNear(eps), AsVector(d_u->light_xpos, 3)));
Pointwise(MjNear(eps, eps), AsVector(d_u->light_xpos, 3)));
EXPECT_THAT(AsVector(d->light_xdir, 3),
Pointwise(DoubleNear(eps), AsVector(d_u->light_xdir, 3)));
Pointwise(MjNear(eps, eps), AsVector(d_u->light_xdir, 3)));
mj_deleteData(d_u);
mj_deleteData(d);
@@ -1618,8 +1632,8 @@ TEST_F(InheritrangeTest, WorksForDegrees) {
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_DOUBLE_EQ(model->actuator_ctrlrange[0], mjPI / 2);
EXPECT_DOUBLE_EQ(model->actuator_ctrlrange[1], mjPI);
EXPECT_MJTNUM_EQ(model->actuator_ctrlrange[0], mjPI / 2);
EXPECT_MJTNUM_EQ(model->actuator_ctrlrange[1], mjPI);
mj_deleteModel(model);
}
@@ -2191,8 +2205,8 @@ TEST_F(SpringrangeTest, DefaultsPropagate) {
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->tendon_lengthspring[0], .2);
EXPECT_EQ(model->tendon_lengthspring[1], .5);
EXPECT_MJTNUM_EQ(model->tendon_lengthspring[0], .2);
EXPECT_MJTNUM_EQ(model->tendon_lengthspring[1], .5);
mj_deleteModel(model);
}
@@ -2262,7 +2276,7 @@ TEST_F(UserObjectsTest, Frame) {
</worldbody>
</mujoco>
)";
constexpr mjtNum eps = 1e-14;
const mjtNum eps = MjTol(1e-14, 1e-5);
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, testing::NotNull()) << error.data();
@@ -2331,7 +2345,7 @@ TEST_F(UserObjectsTest, FrameTransformsLight) {
EXPECT_THAT(m, NotNull()) << error.data();
EXPECT_EQ(m->nlight, 1);
constexpr mjtNum eps = 1e-14;
const mjtNum eps = MjTol(1e-14, 1e-5);
EXPECT_NEAR(m->light_pos[0], -mju_sqrt(.5), eps);
EXPECT_NEAR(m->light_pos[1], 0, eps);
EXPECT_NEAR(m->light_pos[2], 1 + mju_sqrt(.5), eps);
@@ -2628,7 +2642,7 @@ TEST_F(UserObjectsTest, Inertial) {
const mjtNum euler[3] = {3, 4, 5};
mju_euler2Quat(quat, euler, "xyz");
EXPECT_THAT(AsVector(m->body_iquat + 4, 4),
Pointwise(DoubleNear(1e-8), AsVector(quat, 4)));
Pointwise(MjNear(1e-8, 1e-6), AsVector(quat, 4)));
EXPECT_EQ(m->body_mass[2], 2);
EXPECT_THAT(AsVector(m->body_ipos + 6, 3), ElementsAre(1, 2, 3));
@@ -2730,7 +2744,7 @@ TEST_F(OctreeSDFTest, SphereSDF) {
for (double x = -2.0; x <= 2.0; x += 0.5) {
for (double y = -2.0; y <= 2.0; y += 0.5) {
for (double z = -2.0; z <= 2.0; z += 0.5) {
mjtNum p[3] = {x, y, z};
mjtNum p[3] = {(mjtNum)x, (mjtNum)y, (mjtNum)z};
double sdf_dist = mjc_distance(model, data, &sdf, p);
double gt_dist = analyticSdf(p);
@@ -2799,9 +2813,9 @@ TEST_F(OctreeSDFTest, TorusSDF) {
double sum_sq_error = 0.0;
// Test grid of points
for (double x = -2.0; x <= 2.0; x += 0.5) {
for (double y = -2.0; y <= 2.0; y += 0.5) {
for (double z = -2.0; z <= 2.0; z += 0.5) {
for (mjtNum x = -2.0; x <= 2.0; x += 0.5) {
for (mjtNum y = -2.0; y <= 2.0; y += 0.5) {
for (mjtNum z = -2.0; z <= 2.0; z += 0.5) {
mjtNum p[3] = {x, y, z};
double sdf_dist = mjc_distance(model, data, &sdf, p);
double gt_dist = analyticSdf(p);
test/xml/xml_native_reader_test.cc
+15 -14
View File
@@ -1182,9 +1182,9 @@ TEST_F(XMLReaderTest, TendonArmature) {
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, NotNull()) << error.data();
EXPECT_EQ(m->ntendon, 3);
EXPECT_FLOAT_EQ(m->tendon_armature[0], 1.5);
EXPECT_FLOAT_EQ(m->tendon_armature[1], 2.5);
EXPECT_FLOAT_EQ(m->tendon_armature[2], 0);
EXPECT_MJTNUM_EQ(m->tendon_armature[0], 1.5);
EXPECT_MJTNUM_EQ(m->tendon_armature[1], 2.5);
EXPECT_MJTNUM_EQ(m->tendon_armature[2], 0);
mj_deleteModel(m);
}
@@ -1378,18 +1378,19 @@ TEST_F(XMLReaderTest, ParseReplicate) {
// check body positions
mjtNum pos[2] = {0, 0};
constexpr mjtNum tol = MjTol(1e-8, 1e-3);
for (int i = 1; i < 102; ++i) {
mjtNum theta = (i-1) * 1.8 * mjPI / 180;
EXPECT_NEAR(m->body_pos[3*i+0], pos[0] + sin(theta), 1e-8) << i;
EXPECT_NEAR(m->body_pos[3*i+1], pos[1] - cos(theta), 1e-8) << i;
EXPECT_NEAR(m->body_pos[3*i+2], (i-1) * .1, 1e-8);
EXPECT_NEAR(m->body_pos[3*i+0], pos[0] + sin(theta), tol) << i;
EXPECT_NEAR(m->body_pos[3*i+1], pos[1] - cos(theta), tol) << i;
EXPECT_NEAR(m->body_pos[3*i+2], (i-1) * .1, tol);
pos[0] += 3 * cos(theta);
pos[1] += 3 * sin(theta);
}
// check that the final pose is correct
int n = m->nbody-1;
EXPECT_NEAR(m->body_quat[4*n+0], 0, 1e-8);
EXPECT_NEAR(m->body_quat[4*n+0], 0, tol);
EXPECT_EQ(m->body_quat[4*n+1], 0);
EXPECT_EQ(m->body_quat[4*n+2], 0);
EXPECT_EQ(m->body_quat[4*n+3], 1);
@@ -2319,7 +2320,7 @@ TEST_F(HfieldParsingTest, NoData) {
EXPECT_EQ(model->hfield_size[0], 0.5);
EXPECT_EQ(model->hfield_size[1], 0.5);
EXPECT_EQ(model->hfield_size[2], 1);
EXPECT_EQ(model->hfield_size[3], 0.1);
EXPECT_MJTNUM_EQ(model->hfield_size[3], 0.1);
mj_deleteModel(model);
}
@@ -2360,7 +2361,7 @@ TEST_F(HfieldParsingTest, HasData) {
EXPECT_EQ(model->hfield_size[0], 0.5);
EXPECT_EQ(model->hfield_size[1], 0.5);
EXPECT_EQ(model->hfield_size[2], 1);
EXPECT_EQ(model->hfield_size[3], 0.1);
EXPECT_MJTNUM_EQ(model->hfield_size[3], 0.1);
// offset (minimum) and scaling (maximum) from normalizing operation
float offset = 1.0;
@@ -2809,10 +2810,10 @@ TEST_F(ActuatorParseTest, IntvelocityCheckEquivalence) {
EXPECT_DOUBLE_EQ(model->actuator_biasprm[mjNBIAS + 1], -2.5);
EXPECT_DOUBLE_EQ(model->actuator_biasprm[mjNBIAS + 2], 0.0);
// same actrange
EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 0], -1.57);
EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 1], 1.57);
EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 2], -1.57);
EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 3], 1.57);
EXPECT_MJTNUM_EQ(model->actuator_actrange[0 + 0], -1.57);
EXPECT_MJTNUM_EQ(model->actuator_actrange[0 + 1], 1.57);
EXPECT_MJTNUM_EQ(model->actuator_actrange[0 + 2], -1.57);
EXPECT_MJTNUM_EQ(model->actuator_actrange[0 + 3], 1.57);
mj_deleteModel(model);
}
@@ -3047,7 +3048,7 @@ TEST_F(ActuatorParseTest, MusclesParseSmoothdyn) {
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->actuator_dynprm[2], 0.0);
EXPECT_EQ(model->actuator_dynprm[mjNDYN + 2], 0.4);
EXPECT_MJTNUM_EQ(model->actuator_dynprm[mjNDYN + 2], 0.4);
mj_deleteModel(model);
}
test/xml/xml_native_writer_test.cc
+6
View File
@@ -746,6 +746,9 @@ TEST_F(XMLWriterTest, WritesActuatorDefaults) {
}
TEST_F(XMLWriterTest, WritesFrameDefaults) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "Quat values differ in float32 (euler->quat rounding)";
#endif
static constexpr char xml[] = R"(
<mujoco model="test">
<default>
@@ -1184,6 +1187,9 @@ TEST_F(XMLWriterTest, ActdimDefaults) {
}
TEST_F(XMLWriterTest, TrimsDefaults) {
#ifdef mjUSESINGLE
GTEST_SKIP() << "float(0.9) != double(0.9), so default-trimming fails";
#endif
static constexpr char xml[] = R"(
<mujoco>
<worldbody>