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

Add mjENBL_DIAGEXACT for exact constraint diagonal. Fixes #2472

Browse Source 
PiperOrigin-RevId: 916932908
Change-Id: Id23ac39b5cd996afc52990719a4e07c0cc7de600
This commit is contained in:
Yuval Tassa
2026-05-17 16:47:20 -07:00
committed by Copybara-Service
parent 66d764a116
commit 71d1014e70
21 changed files with 234 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_forward_test.cc
+37
View File
@@ -917,6 +917,43 @@ TEST_F(ImplicitIntegratorTest, MidpointEligibility) {
mj_deleteModel(m);
}
// model with degenerate translational inertia
TEST_F(ForwardTest, DegenerateInertia) {
static constexpr char xml[] = R"(
<mujoco>
<option integrator="implicitfast" cone="elliptic">
<flag gravity="disable" diagexact="enable"/>
</option>
<worldbody>
<body name="1" pos="0.05 0.3 0">
<joint name="1" axis="0 1 0"/>
<geom type="capsule" size="0.1 0.5"/>
</body>
<body name="2">
<joint name="2" axis="1 0 0" stiffness="1" springref="90"/>
<geom type="capsule" size="0.1 0.5"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
for (int i = 0; i < 1000; i++) {
mj_step(model, data);
EXPECT_EQ(data->warning[mjWARN_BADQACC].number, 0)
<< "divergence at timestep " << i;
if (data->warning[mjWARN_BADQACC].number != 0) {
break;
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(ForwardTest, ControlClamping) {
static constexpr char xml[] = R"(
<mujoco>
test/engine/engine_inverse_test.cc
+28 -18
View File
@@ -48,28 +48,38 @@ TEST_F(InverseTest, ForwardInverseMatch) {
// solver names for diagnostics
const char* solver_name[] = {"PGS", "CG", "Newton"};
for (mjtSolver solver : {mjSOL_PGS, mjSOL_CG, mjSOL_NEWTON}) {
model->opt.solver = solver;
mj_resetData(model, data);
// simulate, call mj_forward
for (int i = 0; i < kSteps; ++i) {
mj_step(model, data);
for (int diagexact = 0; diagexact < 2; diagexact++) {
if (diagexact) {
model->opt.enableflags |= mjENBL_DIAGEXACT;
} else {
model->opt.enableflags &= ~mjENBL_DIAGEXACT;
}
mj_forward(model, data);
// call built-in testing function
mj_compareFwdInv(model, data);
for (mjtSolver solver : {mjSOL_PGS, mjSOL_CG, mjSOL_NEWTON}) {
model->opt.solver = solver;
mj_resetData(model, data);
// per-solver tolerances
mjtNum epsilon;
switch (solver) {
case mjSOL_PGS: epsilon = MjTol(1e-6, 1e-2); break;
case mjSOL_CG: epsilon = MjTol(1e-3, 1e-1); break;
case mjSOL_NEWTON: epsilon = MjTol(1e-10, 5e-3); break;
// simulate, call mj_forward
for (int i = 0; i < kSteps; ++i) {
mj_step(model, data);
}
mj_forward(model, data);
// call built-in testing function
mj_compareFwdInv(model, data);
// per-solver tolerances
mjtNum epsilon;
switch (solver) {
case mjSOL_PGS: epsilon = MjTol(1e-6, 1e-2); break;
case mjSOL_CG: epsilon = MjTol(1e-3, 1e0); break;
case mjSOL_NEWTON: epsilon = MjTol(1e-10, 1e-2); break;
}
EXPECT_LT(data->solver_fwdinv[0], epsilon)
<< solver_name[solver] << " diagexact=" << diagexact;
EXPECT_LT(data->solver_fwdinv[1], epsilon)
<< solver_name[solver] << " diagexact=" << diagexact;
}
EXPECT_LT(data->solver_fwdinv[0], epsilon) << solver_name[solver];
EXPECT_LT(data->solver_fwdinv[1], epsilon) << solver_name[solver];
}
mj_deleteData(data);