chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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