Add Jdot correction for connect and weld constaints

Measured reduction in constraint violations before/after this change:

| Model | Correction ON (Avg Viol) | Correction OFF (Avg Viol) | Reduction |
| :--- | :--- | :--- | :--- |
| `jdotv_connect_2d.xml` | 3.959e-4 | 1.699e-3 | **76.7%** |
| `jdotv_connect_3d.xml` | 1.399e-3 | 5.493e-3 | **74.5%** |
| `jdotv_weld_3d.xml` | 9.472e-3 | 1.148e-2 | **17.5%** |

PiperOrigin-RevId: 899137525
Change-Id: Ic3e33764ebd64239bab916289c23d80c3da0b51b
This commit is contained in:
Yuval Tassa
2026-04-13 12:47:17 -07:00
committed by Copybara-Service
parent 0c337799bd
commit 412cee2059
13 changed files with 481 additions and 43 deletions
+11 -6
View File
@@ -55,16 +55,21 @@ TEST_F(SolverTest, IslandsEquivalent) {
mjData* data_island = mj_makeData(model);
mjData* data_noisland = mj_makeData(model);
// Below are 3 tolerances associated with 3 different iteration counts,
// they are only moderately tight, 12x higher than x86-64 failure on Linux,
// i.e. in that case the test fails with rtol smaller than {6e-3, 6e-4, 6e-5}.
constexpr int kNumTol = 3;
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.
constexpr int kNumTol = 3;
mjtNum maxiter[kNumTol] = {30, 40, 60};
mjtNum rtol[kNumTol] = {6e-2, 6e-3, 6e-4};
mjtNum rtol[kNumTol] = {
MjTol(6e-2, 7.2e-1),
MjTol(6e-3, 7.2e-2),
MjTol(6e-4, 7.2e-3)
};
for (int i = 0; i < kNumTol; ++i) {
model->opt.iterations = maxiter[i];