Add manual test for CG converence
Also add MJTOL_SCALE to fixture to allow tests to be run with zero tolerance. This is useful when assesing the impact of code changes (A/B comparison of failure values) PiperOrigin-RevId: 924219083 Change-Id: Ifdd09ac850904ca8dd79179930ce738a4b37d284
This commit is contained in:
Yuval Tassa
committed by
Copybara-Service
Copybara-Service
parent
50e823e91c
commit
4358a102cd
@@ -0,0 +1,374 @@
|
||||
// Copyright 2026 DeepMind Technologies Limited
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
// CG solver convergence benchmark.
|
||||
//
|
||||
// Rolls out Newton ground truth on 2humanoid100.xml, then evaluates CG qacc
|
||||
// error. Pass 1: vary iterations with/without warmstart. Pass 2: vary
|
||||
// tolerance. Pass 3: consecutive stepping. No assertions, only data.
|
||||
|
||||
#include <chrono> // NOLINT
|
||||
#include <cstdio>
|
||||
#include <ratio> // NOLINT
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <gmock/gmock.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <mujoco/mujoco.h>
|
||||
#include "test/fixture.h"
|
||||
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
using ::testing::NotNull;
|
||||
|
||||
mjtNum gettm(void) {
|
||||
using Clock = std::chrono::steady_clock;
|
||||
using Microseconds = std::chrono::duration<mjtNum, std::micro>;
|
||||
static const Clock::time_point tm_start = Clock::now();
|
||||
return Microseconds(Clock::now() - tm_start).count();
|
||||
}
|
||||
|
||||
using CgConvergenceTest = MujocoTest;
|
||||
|
||||
TEST_F(CgConvergenceTest, CGConvergence) {
|
||||
static const char* const kPath =
|
||||
"engine/testdata/island/2humanoid100.xml";
|
||||
const std::string xml_path = GetTestDataFilePath(kPath);
|
||||
char error[1024];
|
||||
mjModel* model =
|
||||
mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
|
||||
// disable islands: monolithic solver makes statistics simpler
|
||||
model->opt.disableflags |= mjDSBL_ISLAND;
|
||||
|
||||
int nq = model->nq;
|
||||
int nv = model->nv;
|
||||
|
||||
// stronger sideways gravity
|
||||
model->opt.gravity[0] = -2;
|
||||
model->opt.gravity[1] = -2;
|
||||
model->opt.gravity[2] = -10;
|
||||
|
||||
// configure Newton ground truth: tolerance 0, generous iterations, warmstart
|
||||
model->opt.solver = mjSOL_NEWTON;
|
||||
model->opt.jacobian = mjJAC_SPARSE;
|
||||
model->opt.tolerance = 0;
|
||||
model->opt.iterations = 10;
|
||||
model->opt.disableflags &= ~mjDSBL_WARMSTART;
|
||||
|
||||
mjData* data = mj_makeData(model);
|
||||
mjcb_time = gettm;
|
||||
|
||||
// roll out Newton for kNumSteps, save pre-step state and post-step qacc
|
||||
constexpr int kNumSteps = 1000;
|
||||
std::vector<mjtNum> all_qpos(kNumSteps * nq);
|
||||
std::vector<mjtNum> all_qvel(kNumSteps * nv);
|
||||
std::vector<mjtNum> all_warmstart(kNumSteps * nv);
|
||||
std::vector<mjtNum> all_qacc(kNumSteps * nv);
|
||||
|
||||
for (int i = 0; i < kNumSteps; i++) {
|
||||
// save pre-step state
|
||||
mju_copy(all_qpos.data() + i*nq, data->qpos, nq);
|
||||
mju_copy(all_qvel.data() + i*nv, data->qvel, nv);
|
||||
mju_copy(all_warmstart.data() + i*nv, data->qacc_warmstart, nv);
|
||||
|
||||
// step with Newton
|
||||
mj_step(model, data);
|
||||
|
||||
// save qacc (set by mj_forward inside mj_step)
|
||||
mju_copy(all_qacc.data() + i*nv, data->qacc, nv);
|
||||
}
|
||||
|
||||
// evaluation points: 100 evenly spaced
|
||||
constexpr int kNumEval = 100;
|
||||
constexpr int kStride = kNumSteps / kNumEval;
|
||||
|
||||
// iteration counts to test
|
||||
constexpr int kIterCounts[] = {5, 10, 20, 40, 80, 160};
|
||||
constexpr int kNumIter = sizeof(kIterCounts) / sizeof(kIterCounts[0]);
|
||||
|
||||
// print header
|
||||
std::printf("\nCG Convergence: 2humanoid100.xml\n");
|
||||
std::printf(" %d Newton steps, %d evaluation points\n",
|
||||
kNumSteps, kNumEval);
|
||||
std::printf(" nv = %d, nq = %d\n", nv, nq);
|
||||
std::printf(" metric: ||qacc_cg - qacc_newton|| / ||qacc_newton||\n");
|
||||
|
||||
// table header
|
||||
std::printf("\n Warmstart (tolerance = 0):\n");
|
||||
std::printf(" %6s | %11s | %11s | %10s | %8s\n",
|
||||
"Iters", "Mean Err", "Max Err", "Mean Iters", "LS evals");
|
||||
std::printf(" %s\n",
|
||||
"-------+-------------+-------------+------------+---------");
|
||||
|
||||
// configure CG: full rollout (tolerance 0), warmstart enabled
|
||||
model->opt.solver = mjSOL_CG;
|
||||
model->opt.tolerance = 0;
|
||||
model->opt.disableflags &= ~mjDSBL_WARMSTART;
|
||||
|
||||
for (int c = 0; c < kNumIter; c++) {
|
||||
model->opt.iterations = kIterCounts[c];
|
||||
|
||||
int total_iters = 0;
|
||||
int total_neval = 0;
|
||||
mjtNum sum_rel_err = 0;
|
||||
mjtNum max_rel_err = 0;
|
||||
|
||||
for (int e = 0; e < kNumEval; e++) {
|
||||
int idx = e * kStride;
|
||||
|
||||
// restore state
|
||||
mju_copy(data->qpos, all_qpos.data() + idx*nq, nq);
|
||||
mju_copy(data->qvel, all_qvel.data() + idx*nv, nv);
|
||||
mju_copy(data->qacc_warmstart, all_warmstart.data() + idx*nv, nv);
|
||||
|
||||
// run CG forward
|
||||
mj_forward(model, data);
|
||||
int niter = data->solver_niter[0];
|
||||
total_iters += niter;
|
||||
for (int j = 0; j < niter && j < mjNSOLVER; j++) {
|
||||
total_neval += data->solver[j].neval;
|
||||
}
|
||||
|
||||
// compute relative error
|
||||
mjtNum newton_norm = mju_norm(all_qacc.data() + idx*nv, nv);
|
||||
mjtNum err = 0;
|
||||
for (int j = 0; j < nv; j++) {
|
||||
mjtNum diff = data->qacc[j] - all_qacc[idx*nv + j];
|
||||
err += diff * diff;
|
||||
}
|
||||
mjtNum rel_err = mju_sqrt(err) / mju_max(newton_norm, 1e-10);
|
||||
sum_rel_err += rel_err;
|
||||
if (rel_err > max_rel_err) {
|
||||
max_rel_err = rel_err;
|
||||
}
|
||||
}
|
||||
|
||||
mjtNum mean_iters = static_cast<mjtNum>(total_iters) / kNumEval;
|
||||
std::printf(" %6d | %11.4e | %11.4e | %10.2f | %8d\n",
|
||||
kIterCounts[c], sum_rel_err / kNumEval, max_rel_err,
|
||||
mean_iters, total_neval);
|
||||
}
|
||||
|
||||
std::printf(" %s\n",
|
||||
"-------+-------------+-------------+------------+---------");
|
||||
|
||||
// --- no warmstart (tolerance = 0) ---
|
||||
std::printf("\n No warmstart (tolerance = 0):\n");
|
||||
std::printf(" %6s | %11s | %11s | %10s | %8s\n",
|
||||
"Iters", "Mean Err", "Max Err", "Mean Iters", "LS evals");
|
||||
std::printf(" %s\n",
|
||||
"-------+-------------+-------------+------------+---------");
|
||||
|
||||
model->opt.disableflags |= mjDSBL_WARMSTART;
|
||||
|
||||
for (int c = 0; c < kNumIter; c++) {
|
||||
model->opt.iterations = kIterCounts[c];
|
||||
|
||||
int total_iters = 0;
|
||||
int total_neval = 0;
|
||||
mjtNum sum_rel_err = 0;
|
||||
mjtNum max_rel_err = 0;
|
||||
|
||||
for (int e = 0; e < kNumEval; e++) {
|
||||
int idx = e * kStride;
|
||||
|
||||
mju_copy(data->qpos, all_qpos.data() + idx*nq, nq);
|
||||
mju_copy(data->qvel, all_qvel.data() + idx*nv, nv);
|
||||
|
||||
mj_forward(model, data);
|
||||
int niter = data->solver_niter[0];
|
||||
total_iters += niter;
|
||||
for (int j = 0; j < niter && j < mjNSOLVER; j++) {
|
||||
total_neval += data->solver[j].neval;
|
||||
}
|
||||
|
||||
mjtNum newton_norm = mju_norm(all_qacc.data() + idx*nv, nv);
|
||||
mjtNum err = 0;
|
||||
for (int j = 0; j < nv; j++) {
|
||||
mjtNum diff = data->qacc[j] - all_qacc[idx*nv + j];
|
||||
err += diff * diff;
|
||||
}
|
||||
mjtNum rel_err = mju_sqrt(err) / mju_max(newton_norm, 1e-10);
|
||||
sum_rel_err += rel_err;
|
||||
if (rel_err > max_rel_err) {
|
||||
max_rel_err = rel_err;
|
||||
}
|
||||
}
|
||||
|
||||
mjtNum mean_iters = static_cast<mjtNum>(total_iters) / kNumEval;
|
||||
std::printf(" %6d | %11.4e | %11.4e | %10.2f | %8d\n",
|
||||
kIterCounts[c], sum_rel_err / kNumEval, max_rel_err,
|
||||
mean_iters, total_neval);
|
||||
}
|
||||
|
||||
std::printf(" %s\n",
|
||||
"-------+-------------+-------------+------------+---------");
|
||||
|
||||
// --- tolerance sweep (iterations = 100, warmstart) ---
|
||||
std::printf("\n Tolerance sweep (iterations = 100, warmstart):\n");
|
||||
std::printf(" %10s | %11s | %11s | %9s | %10s | %11s | %8s\n",
|
||||
"Tol", "Mean Err", "Max Err", "Mean Iters", "Max Iters",
|
||||
"Solver us", "LS evals");
|
||||
std::printf(" %s\n",
|
||||
"-----------+-------------+-------------+"
|
||||
"------------+------------+-------------+---------");
|
||||
|
||||
model->opt.solver = mjSOL_CG;
|
||||
model->opt.iterations = 100;
|
||||
model->opt.disableflags &= ~mjDSBL_WARMSTART;
|
||||
|
||||
constexpr mjtNum kTolValues[] = {1e-4, 1e-6, 1e-8, 1e-10, 1e-12, 0};
|
||||
constexpr int kNumTol = sizeof(kTolValues) / sizeof(kTolValues[0]);
|
||||
|
||||
mjtNum total_solver_time = 0;
|
||||
int grand_total_iters = 0;
|
||||
|
||||
for (int c = 0; c < kNumTol; c++) {
|
||||
for (int i = 0; i < mjNTIMER; i++) {
|
||||
data->timer[i].duration = 0;
|
||||
data->timer[i].number = 0;
|
||||
}
|
||||
|
||||
model->opt.tolerance = kTolValues[c];
|
||||
|
||||
int total_iters = 0;
|
||||
int max_iters = 0;
|
||||
int total_neval = 0;
|
||||
mjtNum sum_rel_err = 0;
|
||||
mjtNum max_rel_err = 0;
|
||||
|
||||
for (int e = 0; e < kNumEval; e++) {
|
||||
int idx = e * kStride;
|
||||
|
||||
mju_copy(data->qpos, all_qpos.data() + idx*nq, nq);
|
||||
mju_copy(data->qvel, all_qvel.data() + idx*nv, nv);
|
||||
mju_copy(data->qacc_warmstart, all_warmstart.data() + idx*nv, nv);
|
||||
|
||||
mj_forward(model, data);
|
||||
int niter = data->solver_niter[0];
|
||||
total_iters += niter;
|
||||
if (niter > max_iters) {
|
||||
max_iters = niter;
|
||||
}
|
||||
for (int j = 0; j < niter && j < mjNSOLVER; j++) {
|
||||
total_neval += data->solver[j].neval;
|
||||
}
|
||||
|
||||
mjtNum newton_norm = mju_norm(all_qacc.data() + idx*nv, nv);
|
||||
mjtNum err = 0;
|
||||
for (int j = 0; j < nv; j++) {
|
||||
mjtNum diff = data->qacc[j] - all_qacc[idx*nv + j];
|
||||
err += diff * diff;
|
||||
}
|
||||
mjtNum rel_err = mju_sqrt(err) / mju_max(newton_norm, 1e-10);
|
||||
sum_rel_err += rel_err;
|
||||
if (rel_err > max_rel_err) {
|
||||
max_rel_err = rel_err;
|
||||
}
|
||||
}
|
||||
|
||||
mjtNum solver_time = data->timer[mjTIMER_CONSTRAINT].duration;
|
||||
total_solver_time += solver_time;
|
||||
grand_total_iters += total_iters;
|
||||
|
||||
mjtNum mean_iters = static_cast<mjtNum>(total_iters) / kNumEval;
|
||||
if (kTolValues[c] > 0) {
|
||||
std::printf(
|
||||
" %10.0e | %11.4e | %11.4e | %10.2f | %10d | %11.2f | %8d\n",
|
||||
kTolValues[c], sum_rel_err / kNumEval, max_rel_err,
|
||||
mean_iters, max_iters, solver_time, total_neval);
|
||||
} else {
|
||||
std::printf(
|
||||
" %10s | %11.4e | %11.4e | %10.2f | %10d | %11.2f | %8d\n",
|
||||
"0", sum_rel_err / kNumEval, max_rel_err,
|
||||
mean_iters, max_iters, solver_time, total_neval);
|
||||
}
|
||||
}
|
||||
|
||||
mjtNum overall_avg = grand_total_iters > 0
|
||||
? total_solver_time / grand_total_iters : 0.0;
|
||||
std::printf(" %s\n",
|
||||
"-----------+-------------+-------------+"
|
||||
"------------+------------+-------------+---------");
|
||||
std::printf(" Total solver time: %.2f us, avg time per iter: %.4f us\n",
|
||||
total_solver_time, overall_avg);
|
||||
|
||||
// --- third pass: consecutive stepping (mini-testspeed) ---
|
||||
// uses model defaults: tolerance = 1e-8, iterations = 100
|
||||
std::printf("\n Pipeline mode (consecutive mj_step, tolerance = 1e-8):\n");
|
||||
|
||||
model->opt.solver = mjSOL_CG;
|
||||
model->opt.tolerance = 1e-8;
|
||||
model->opt.iterations = 100;
|
||||
model->opt.disableflags &= ~mjDSBL_WARMSTART;
|
||||
|
||||
// reset data to initial state
|
||||
mj_resetData(model, data);
|
||||
|
||||
// clear timers
|
||||
for (int i = 0; i < mjNTIMER; i++) {
|
||||
data->timer[i].duration = 0;
|
||||
data->timer[i].number = 0;
|
||||
}
|
||||
|
||||
// run consecutive steps
|
||||
constexpr int kPipeSteps = 1000;
|
||||
int pipe_total_iters = 0;
|
||||
int pipe_total_neval = 0;
|
||||
for (int i = 0; i < kPipeSteps; i++) {
|
||||
mj_step(model, data);
|
||||
int niter = data->solver_niter[0];
|
||||
pipe_total_iters += niter;
|
||||
for (int j = 0; j < niter && j < mjNSOLVER; j++) {
|
||||
pipe_total_neval += data->solver[j].neval;
|
||||
}
|
||||
}
|
||||
|
||||
mjtNum pipe_constraint = data->timer[mjTIMER_CONSTRAINT].duration;
|
||||
mjtNum pipe_step = data->timer[mjTIMER_STEP].duration;
|
||||
int pipe_step_count = data->timer[mjTIMER_STEP].number;
|
||||
mjtNum us_per_step = pipe_step_count > 0 ? pipe_step / pipe_step_count : 0;
|
||||
mjtNum constraint_per_step = pipe_step_count > 0
|
||||
? pipe_constraint / pipe_step_count : 0;
|
||||
mjtNum iters_per_step = pipe_step_count > 0
|
||||
? static_cast<mjtNum>(pipe_total_iters) / pipe_step_count : 0;
|
||||
mjtNum steps_per_sec = us_per_step > 0 ? 1e6 / us_per_step : 0;
|
||||
|
||||
std::printf(" %d steps, nv = %d\n", kPipeSteps, nv);
|
||||
std::printf(" Steps/s : %.0f\n", steps_per_sec);
|
||||
std::printf(" us/step (total) : %.1f\n", us_per_step);
|
||||
std::printf(" us/step (constr) : %.1f (%.1f%%)\n",
|
||||
constraint_per_step,
|
||||
us_per_step > 0 ? 100*constraint_per_step/us_per_step : 0.0);
|
||||
std::printf(" CG iters/step : %.2f\n", iters_per_step);
|
||||
std::printf(" LS evals/step : %.2f\n",
|
||||
pipe_step_count > 0
|
||||
? static_cast<mjtNum>(pipe_total_neval) / pipe_step_count
|
||||
: 0.0);
|
||||
std::printf(" us/iter : %.2f\n",
|
||||
pipe_total_iters > 0
|
||||
? pipe_constraint / pipe_total_iters : 0.0);
|
||||
|
||||
std::printf("\n");
|
||||
mjcb_time = nullptr;
|
||||
mj_deleteData(data);
|
||||
mj_deleteModel(model);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace mujoco
|
||||
@@ -176,7 +176,7 @@ TEST_F(AngMomMatTest, CompareAngMomMats) {
|
||||
mj_angmomMat(model, data, angmom_mat, bodyid);
|
||||
|
||||
// compute the angular momentum matrix using finite differences
|
||||
static constexpr mjtNum eps = MjTol(1e-6, 1e-3);
|
||||
static const 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);
|
||||
@@ -505,7 +505,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
|
||||
constexpr mjtNum h = MjTol(1e-7, 5e-4);
|
||||
const mjtNum h = MjTol(1e-7, 5e-4);
|
||||
mj_integratePos(model, data->qpos, data->qvel, h);
|
||||
mj_kinematics(model, data);
|
||||
mj_comPos(model, data);
|
||||
|
||||
@@ -297,7 +297,7 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
|
||||
d1->qpos[6] = 0.5;
|
||||
|
||||
// tolerance for floating point numbers
|
||||
constexpr mjtNum tol = MjTol(1e-14, 1e-5);
|
||||
const mjtNum tol = MjTol(1e-14, 1e-5);
|
||||
|
||||
EXPECT_EQ(m1->nv, m2->nv);
|
||||
|
||||
@@ -552,7 +552,7 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) {
|
||||
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
|
||||
}
|
||||
}
|
||||
constexpr mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-5);
|
||||
const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-5);
|
||||
EXPECT_NEAR(4*energy/volume, 2*scale*scale, tol);
|
||||
}
|
||||
}
|
||||
@@ -603,7 +603,7 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
|
||||
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
|
||||
}
|
||||
}
|
||||
constexpr mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-4);
|
||||
const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-4);
|
||||
EXPECT_NEAR(energy/volume, 3*scale*scale, tol);
|
||||
}
|
||||
}
|
||||
@@ -1019,7 +1019,7 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
|
||||
mj_forward(m, d);
|
||||
|
||||
// spring forces should still be zero after rigid rotation
|
||||
constexpr mjtNum tol = MjTol(1e-6, 1e-3);
|
||||
const mjtNum tol = MjTol(1e-6, 1e-3);
|
||||
for (int i = 0; i < m->nv; i++) {
|
||||
EXPECT_NEAR(d->qfrc_spring[i], 0, tol)
|
||||
<< "nonzero spring force at DOF " << i << " after rigid rotation";
|
||||
|
||||
@@ -26,9 +26,8 @@
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
using ::testing::NotNull;
|
||||
using ::testing::Pointwise;
|
||||
using ::std::max;
|
||||
using ::testing::NotNull;
|
||||
|
||||
using SolverTest = MujocoTest;
|
||||
|
||||
@@ -60,16 +59,14 @@ TEST_F(SolverTest, IslandsEquivalent) {
|
||||
mjtNum maxiter[kNumTol] = {30, 40, 60};
|
||||
// Below are 3 tolerances associated with 3 different iteration counts.
|
||||
// Tolerances are set to be ~12x higher than failure thresholds.
|
||||
// For float32, failure thresholds are ~6000x larger than for float64.
|
||||
// Line 99 adds a 500x factor for float32, so we need another ~12x in rtol.
|
||||
// The point of this test is to show that CG convergence is actually not very
|
||||
// precise, simply changing whether islands are used changes the solution by
|
||||
// quite a lot, even at high iteration count and zero {ls_}tolerance.
|
||||
// Increasing the iteration count higher than 60 does not improve convergence.
|
||||
mjtNum rtol[kNumTol] = {
|
||||
MjTol(6e-2, 7.2e-1),
|
||||
MjTol(6e-3, 7.2e-2),
|
||||
MjTol(6e-4, 7.2e-3)
|
||||
MjTol(1e-1, 2),
|
||||
MjTol(3e-2, 1),
|
||||
MjTol(1.5e-4, 3.6)
|
||||
};
|
||||
|
||||
for (int i = 0; i < kNumTol; ++i) {
|
||||
@@ -85,6 +82,14 @@ TEST_F(SolverTest, IslandsEquivalent) {
|
||||
model->opt.disableflags &= ~mjDSBL_WARMSTART;
|
||||
}
|
||||
|
||||
mjtNum max_ratio = 0;
|
||||
mjtNum worst_diff = 0;
|
||||
mjtNum worst_scale = 1.0;
|
||||
mjtNum worst_expected = 0;
|
||||
mjtNum worst_actual = 0;
|
||||
std::string worst_time = "";
|
||||
int worst_dof = -1;
|
||||
|
||||
while (data_noisland->time < .1) {
|
||||
mj_getState(model, data_noisland, state, mjSTATE_INTEGRATION);
|
||||
mj_setState(model, data_island, state, mjSTATE_INTEGRATION);
|
||||
@@ -95,22 +100,38 @@ TEST_F(SolverTest, IslandsEquivalent) {
|
||||
model->opt.disableflags |= mjDSBL_ISLAND; // disable islands
|
||||
mj_forward(model, data_noisland);
|
||||
|
||||
auto time = std::to_string(data_noisland->time);
|
||||
for (int j = 0; j < nv; j++) {
|
||||
// increase tolerance for large elements
|
||||
mjtNum diff = std::abs(data_noisland->qacc[j] - data_island->qacc[j]);
|
||||
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'
|
||||
<< "rtol: " << scale * rtol[i];
|
||||
std::abs(data_island->qacc[j]));
|
||||
mjtNum ratio = diff / scale;
|
||||
if (ratio > max_ratio) {
|
||||
max_ratio = ratio;
|
||||
worst_diff = diff;
|
||||
worst_scale = scale;
|
||||
worst_expected = data_island->qacc[j];
|
||||
worst_actual = data_noisland->qacc[j];
|
||||
worst_time = std::to_string(data_noisland->time);
|
||||
worst_dof = j;
|
||||
}
|
||||
}
|
||||
|
||||
mj_step(model, data_noisland);
|
||||
}
|
||||
|
||||
// Assert once per condition with the worst offender.
|
||||
// rtol[i] is already scaled by MjTolScale() at initialization.
|
||||
mjtNum allowed_tol = worst_scale * rtol[i];
|
||||
EXPECT_NEAR(worst_actual, worst_expected, allowed_tol)
|
||||
<< "Worst offender info:\n"
|
||||
<< "time: " << worst_time << '\n'
|
||||
<< "dof: " << worst_dof << '\n'
|
||||
<< "maxiter: " << maxiter[i] << '\n'
|
||||
<< "coldstart: " << coldstart << '\n'
|
||||
<< "rtol: " << worst_scale * rtol[i] << '\n'
|
||||
<< "actual diff: " << worst_diff << " (allowed: " << allowed_tol
|
||||
<< ")";
|
||||
}
|
||||
}
|
||||
|
||||
@@ -161,16 +182,36 @@ TEST_F(SolverTest, IslandsEquivalentForward) {
|
||||
model->opt.disableflags &= ~mjDSBL_ISLAND; // enable islands
|
||||
mj_forward(model, data_island);
|
||||
|
||||
mjtNum max_diff = 0;
|
||||
mjtNum worst_expected = 0;
|
||||
mjtNum worst_actual = 0;
|
||||
int worst_idx = -1;
|
||||
mjtNum scale = 0.5 * (mju_norm(data_noisland->qacc, nv) +
|
||||
mju_norm(data_island->qacc, nv));
|
||||
mjtNum tol = scale * (solver == mjSOL_CG ? 1e-6 : 1e-8);
|
||||
EXPECT_THAT(AsVector(data_island->qacc, nv),
|
||||
Pointwise(MjNear(scale * tol, 500 * scale * tol),
|
||||
AsVector(data_noisland->qacc, nv)))
|
||||
mjtNum rtol = solver == mjSOL_CG ? MjTol(1e-6, 1e-2)
|
||||
: MjTol(1e-13, 1e-3);
|
||||
mjtNum worst_allowed = scale * rtol;
|
||||
|
||||
for (int j = 0; j < nv; j++) {
|
||||
mjtNum diff =
|
||||
std::abs(data_island->qacc[j] - data_noisland->qacc[j]);
|
||||
if (diff > max_diff) {
|
||||
max_diff = diff;
|
||||
worst_expected = data_noisland->qacc[j];
|
||||
worst_actual = data_island->qacc[j];
|
||||
worst_idx = j;
|
||||
}
|
||||
}
|
||||
|
||||
EXPECT_NEAR(worst_actual, worst_expected, worst_allowed)
|
||||
<< "Worst offender in IslandsEquivalentForward:\n"
|
||||
<< "idx: " << worst_idx << '\n'
|
||||
<< "warmstart: " << warmstart << '\n'
|
||||
<< "jacobian: " << (jacobian ? "sparse" : "dense") << '\n'
|
||||
<< "solver: " << (solver == mjSOL_CG ? "CG" : "Newton") << '\n'
|
||||
<< "cone: " << (cone == 1 ? "elliptic" : "pyramidal");
|
||||
<< "cone: " << (cone == 1 ? "elliptic" : "pyramidal") << '\n'
|
||||
<< "actual diff: " << max_diff << " (allowed: " << worst_allowed
|
||||
<< ")";
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -183,15 +224,14 @@ TEST_F(SolverTest, IslandsEquivalentForward) {
|
||||
|
||||
TEST_F(SolverTest, SolversEquivalent) {
|
||||
struct SolverTolerances {
|
||||
double newton;
|
||||
double cg;
|
||||
double pgs_pyramidal;
|
||||
double pgs_elliptic;
|
||||
mjtNum newton;
|
||||
mjtNum cg;
|
||||
mjtNum pgs_pyramidal;
|
||||
mjtNum pgs_elliptic;
|
||||
};
|
||||
|
||||
// Base relative tolerances are factor of 10 above failure thresholds
|
||||
// on Linux, clang, x86-64 (i.e., test just passes with tol_multiplier = 1)
|
||||
// TODO: Get float32 tolerances
|
||||
// Relative tolerances: 10x above failure thresholds on Linux, clang, x86-64.
|
||||
// MjTol(f64, f32) selects the appropriate tolerance for the current build.
|
||||
const struct {
|
||||
const char* path;
|
||||
SolverTolerances tolerances;
|
||||
@@ -199,18 +239,18 @@ TEST_F(SolverTest, SolversEquivalent) {
|
||||
{.path = kModelPath,
|
||||
.tolerances =
|
||||
{
|
||||
.newton = 1e-15,
|
||||
.cg = 1e-7,
|
||||
.pgs_pyramidal = 1e-14,
|
||||
.pgs_elliptic = 1e-3,
|
||||
.newton = MjTol(1e-13, 1e-5),
|
||||
.cg = MjTol(1e-5, 1e-2),
|
||||
.pgs_pyramidal = MjTol(1e-12, 1e-5),
|
||||
.pgs_elliptic = MjTol(1e-3, 1e-2),
|
||||
}},
|
||||
{.path = kHumanoidPath,
|
||||
.tolerances =
|
||||
{
|
||||
.newton = 1e-15,
|
||||
.cg = 1e-7,
|
||||
.pgs_pyramidal = 1e-6,
|
||||
.pgs_elliptic = 1e-9,
|
||||
.newton = MjTol(1e-13, 1e-5),
|
||||
.cg = MjTol(1e-5, 1e-2),
|
||||
.pgs_pyramidal = MjTol(1e-5, 1e-5),
|
||||
.pgs_elliptic = MjTol(1e-8, 1e-4),
|
||||
}},
|
||||
};
|
||||
|
||||
@@ -241,7 +281,7 @@ TEST_F(SolverTest, SolversEquivalent) {
|
||||
mjtNum scale = mju_norm(data_truth->qfrc_constraint, nv);
|
||||
|
||||
for (mjtSolver solver : {mjSOL_NEWTON, mjSOL_CG, mjSOL_PGS}) {
|
||||
double rtol;
|
||||
mjtNum rtol;
|
||||
switch (solver) {
|
||||
case mjSOL_NEWTON:
|
||||
rtol = config.tolerances.newton;
|
||||
@@ -255,9 +295,7 @@ TEST_F(SolverTest, SolversEquivalent) {
|
||||
break;
|
||||
}
|
||||
|
||||
// increase base tolerance to avoid test flakiness
|
||||
double tol_multiplier = 1e2;
|
||||
double tolerance = scale * rtol * tol_multiplier;
|
||||
mjtNum tolerance = scale * rtol;
|
||||
|
||||
for (mjtJacobian jacobian : {mjJAC_DENSE, mjJAC_SPARSE}) {
|
||||
model->opt.solver = solver;
|
||||
@@ -274,15 +312,31 @@ TEST_F(SolverTest, SolversEquivalent) {
|
||||
const char* jacobian_str =
|
||||
(jacobian == mjJAC_DENSE ? "dense" : "sparse");
|
||||
|
||||
EXPECT_THAT(AsVector(data->qfrc_constraint, nv),
|
||||
Pointwise(MjNear(tolerance,
|
||||
max(1e-1, 1000 * tolerance)),
|
||||
AsVector(data_truth->qfrc_constraint, nv)))
|
||||
mjtNum max_diff = 0;
|
||||
mjtNum worst_expected = 0;
|
||||
mjtNum worst_actual = 0;
|
||||
int worst_idx = -1;
|
||||
|
||||
for (int j = 0; j < nv; j++) {
|
||||
mjtNum diff = std::abs(data->qfrc_constraint[j] -
|
||||
data_truth->qfrc_constraint[j]);
|
||||
if (diff > max_diff) {
|
||||
max_diff = diff;
|
||||
worst_expected = data_truth->qfrc_constraint[j];
|
||||
worst_actual = data->qfrc_constraint[j];
|
||||
worst_idx = j;
|
||||
}
|
||||
}
|
||||
|
||||
EXPECT_NEAR(worst_actual, worst_expected, tolerance)
|
||||
<< "Worst offender in SolversEquivalent:\n"
|
||||
<< "idx: " << worst_idx << '\n'
|
||||
<< "model: " << config.path << "\n"
|
||||
<< "cone: " << cone_str << "\n"
|
||||
<< "solver: " << solver_str << "\n"
|
||||
<< "jacobian: " << jacobian_str << "\n"
|
||||
<< "tolerance: " << tolerance;
|
||||
<< "actual diff: " << max_diff << " (allowed: " << tolerance
|
||||
<< ")";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -67,8 +67,8 @@ TEST_F(UtilMiscTest, Sigmoid) {
|
||||
EXPECT_EQ(mju_sigmoid(2), 1);
|
||||
|
||||
// epsilon for finite-differencing
|
||||
constexpr mjtNum dx = MjTol(1e-7, 1e-3);
|
||||
constexpr mjtNum fd_tol = MjTol(1e-7, 1e-3);
|
||||
const mjtNum dx = MjTol(1e-7, 1e-3);
|
||||
const mjtNum fd_tol = MjTol(1e-7, 1e-3);
|
||||
|
||||
// derivative at 0
|
||||
mjtNum dy_dx_0 = (mju_sigmoid(0 + dx) - mju_sigmoid(0)) / dx;
|
||||
|
||||
@@ -93,8 +93,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) {
|
||||
constexpr mjtNum eps = MjTol(1e-4, 5e-2); // epsilon used for nudging
|
||||
constexpr mjtNum threshold = MjTol(0, -2e-3); // comparison threshold
|
||||
const mjtNum eps = MjTol(1e-4, 5e-2); // epsilon used for nudging
|
||||
const mjtNum threshold = MjTol(0, -2e-3); // comparison threshold
|
||||
bool is_minimum = true;
|
||||
mjtNum* res_nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*n);
|
||||
|
||||
|
||||
@@ -121,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 constexpr mjtNum eps = MjTol(1e-15, 1e-5);
|
||||
static const mjtNum eps = MjTol(1e-15, 1e-5);
|
||||
for (auto vec : vecs) {
|
||||
// Unit-normalize the vector
|
||||
mju_normalize3(vec);
|
||||
|
||||
+25
-15
@@ -16,7 +16,7 @@
|
||||
#define MUJOCO_TEST_FIXTURE_H_
|
||||
|
||||
#include <csetjmp>
|
||||
#include <cstdio> // IWYU pragma: keep
|
||||
#include <cstdio> // IWYU pragma: keep
|
||||
#include <cstdlib> // IWYU pragma: keep
|
||||
#include <cstring>
|
||||
#include <iomanip>
|
||||
@@ -35,51 +35,63 @@
|
||||
|
||||
extern "C" {
|
||||
MJAPI void _mjPRIVATE__set_tls_error_fn(decltype(mju_user_error));
|
||||
MJAPI decltype(mju_user_error) _mjPRIVATE__get_tls_error_fn();
|
||||
MJAPI decltype(mju_user_error) _mjPRIVATE__get_tls_error_fn();
|
||||
}
|
||||
|
||||
namespace mujoco {
|
||||
|
||||
// Runtime scale factor for test tolerances, controlled by MJTOL_SCALE env var.
|
||||
// Set MJTOL_SCALE=0 to run tests with zero tolerance and see actual residuals.
|
||||
inline mjtNum MjTolScale() {
|
||||
static const mjtNum scale = []() {
|
||||
const char* env = std::getenv("MJTOL_SCALE");
|
||||
return env ? std::atof(env) : 1.0;
|
||||
}();
|
||||
return scale;
|
||||
}
|
||||
|
||||
// Precision-aware GMock matcher. Use instead of DoubleNear/FloatNear.
|
||||
// Under double builds, uses double_tol. Under float builds, uses float_tol.
|
||||
// Scaled by MJTOL_SCALE env var (default 1.0).
|
||||
template <typename T1, typename T2>
|
||||
inline auto MjNear(T1 double_tol, T2 float_tol) {
|
||||
#ifdef mjUSESINGLE
|
||||
return ::testing::FloatNear(static_cast<float>(float_tol));
|
||||
return ::testing::FloatNear(static_cast<float>(float_tol) * MjTolScale());
|
||||
#else
|
||||
return ::testing::DoubleNear(static_cast<double>(double_tol));
|
||||
return ::testing::DoubleNear(static_cast<double>(double_tol) * MjTolScale());
|
||||
#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.
|
||||
// Scaled by MJTOL_SCALE env var (default 1.0).
|
||||
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));
|
||||
static_cast<float>(float_tol) * MjTolScale());
|
||||
#else
|
||||
return ::testing::DoubleNear(static_cast<double>(target),
|
||||
static_cast<double>(double_tol));
|
||||
static_cast<double>(double_tol) * MjTolScale());
|
||||
#endif
|
||||
}
|
||||
|
||||
// Precision-aware tolerance for EXPECT_NEAR.
|
||||
// Scaled by MJTOL_SCALE env var (default 1.0).
|
||||
template <typename T1, typename T2>
|
||||
constexpr mjtNum MjTol(T1 double_tol, T2 float_tol) {
|
||||
inline mjtNum MjTol(T1 double_tol, T2 float_tol) {
|
||||
#ifdef mjUSESINGLE
|
||||
return static_cast<mjtNum>(float_tol);
|
||||
return static_cast<mjtNum>(float_tol) * MjTolScale();
|
||||
#else
|
||||
return static_cast<mjtNum>(double_tol);
|
||||
return static_cast<mjtNum>(double_tol) * MjTolScale();
|
||||
#endif
|
||||
}
|
||||
|
||||
// Precision-aware equality assertion: 4 ULPs in either precision.
|
||||
#ifdef mjUSESINGLE
|
||||
#define EXPECT_MJTNUM_EQ(a, b) EXPECT_FLOAT_EQ(a, b)
|
||||
#define EXPECT_MJTNUM_EQ(a, b) EXPECT_FLOAT_EQ(a, b)
|
||||
#else
|
||||
#define EXPECT_MJTNUM_EQ(a, b) EXPECT_DOUBLE_EQ(a, b)
|
||||
#define EXPECT_MJTNUM_EQ(a, b) EXPECT_DOUBLE_EQ(a, b)
|
||||
#endif
|
||||
|
||||
// Installs and uninstalls error callbacks on MuJoCo that fail the currently
|
||||
@@ -187,7 +199,7 @@ inline void PrintMatrix(const mjtNum* mat, int nrow, int ncol, int p = 5,
|
||||
std::cerr << name << "\n";
|
||||
for (int r = 0; r < nrow; r++) {
|
||||
for (int c = 0; c < ncol; c++) {
|
||||
mjtNum val = mat[c + r*ncol];
|
||||
mjtNum val = mat[c + r * ncol];
|
||||
if (val) {
|
||||
std::cerr << std::fixed << std::setw(5 + p) << val << " ";
|
||||
} else {
|
||||
@@ -232,7 +244,6 @@ class MockFilesystem {
|
||||
const unsigned char** buffer) const;
|
||||
std::string FullPath(const std::string& path) const;
|
||||
|
||||
|
||||
private:
|
||||
std::string StripPrefix(const char* path) const;
|
||||
static std::string PathReduce(const std::string& current_dir,
|
||||
@@ -244,6 +255,5 @@ class MockFilesystem {
|
||||
std::string dir_; // current directory
|
||||
};
|
||||
|
||||
|
||||
} // namespace mujoco
|
||||
#endif // MUJOCO_TEST_FIXTURE_H_
|
||||
|
||||
@@ -42,7 +42,7 @@ TEST_F(PipelineTest, SparseDenseEquivalent) {
|
||||
ASSERT_THAT(model, NotNull()) << error;
|
||||
mjData* data = mj_makeData(model);
|
||||
|
||||
constexpr mjtNum tol = MjTol(1e-11, 1e-4);
|
||||
const 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};
|
||||
|
||||
@@ -31,7 +31,7 @@
|
||||
namespace mujoco {
|
||||
namespace {
|
||||
|
||||
constexpr double kInertiaTol = MjTol(1e-6, 1e-6);
|
||||
const double kInertiaTol = MjTol(1e-6, 1e-6);
|
||||
|
||||
using std::string;
|
||||
using ::testing::ElementsAre;
|
||||
|
||||
@@ -1378,7 +1378,7 @@ TEST_F(XMLReaderTest, ParseReplicate) {
|
||||
|
||||
// check body positions
|
||||
mjtNum pos[2] = {0, 0};
|
||||
constexpr mjtNum tol = MjTol(1e-8, 1e-3);
|
||||
const 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), tol) << i;
|
||||
|
||||
Reference in New Issue
Block a user