Improve CG solver precision and stability under float32 via line search refactor

Fixes #2313

This CL combines two complementary improvements to the Conjugate Gradient (CG) solver, significantly improving numerical precision, stability, and efficiency, particularly in single precision (float32).

1. Line Search Cost Evaluation Refactor: Previously, solver improvement was calculated by subtracting absolute costs: cost(alpha) - cost(0). In highly converged states or single precision, this is susceptible to catastrophic cancellation. We refactor PrimalSearch to compute the cost delta directly, dramatically improving precision.

2. Improved Solver Termination Condition: Near the float32 precision limit, line search deltas can occasionally be slightly negative due to numerical noise. Previously, any value below m->opt.tolerance (including negative values) triggered termination, halting the solver and locking in destabilizing steps. We update the termination condition to require positive improvement (0 < improvement < m->opt.tolerance), allowing the solver to continue iterating and recover stability.

Together, these changes yield substantial improvements, see reduced tolerances herein.

PiperOrigin-RevId: 924229669
Change-Id: Ic0bbefaed090f3a8b1e79ab8d45422c3e86fb56c

test/engine/engine_core_smooth_test.cc

@@ -408,7 +408,7 @@ void TestConnect(const char* const filepath) {
   }
   for (int i=0; i < 3; i++) {
     EXPECT_THAT(data->sensordata[i] - model->sensor_user[i],
-                MjNear(0, 1e-6, 1e-4));
+                MjNear(0, 1e-6, 2e-4));
   }
   mj_deleteData(data);
   mj_deleteModel(model);

test/engine/engine_inverse_test.cc

@@ -72,7 +72,7 @@ TEST_F(InverseTest, ForwardInverseMatch) {
       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_CG:     epsilon = MjTol(1e-9, 1e-1);   break;
       case mjSOL_NEWTON: epsilon = MjTol(1e-10, 1e-2);  break;
       }
       EXPECT_LT(data->solver_fwdinv[0], epsilon)

test/engine/engine_sensor_test.cc

@@ -973,7 +973,7 @@ TEST_F(SensorTest, ContactNet) {
       mj_applyFT(model, data, force, torque, point, b1, qfrc.data());
 
       // compare
-      EXPECT_THAT(qfrc, Pointwise(MjNear(1e-6, 1e-4), qfrc_expected));
+      EXPECT_THAT(qfrc, Pointwise(MjNear(1e-6, 2e-4), qfrc_expected));
 
       // check net force, sensor returns body2 -> body1
       vector net21 = GetSensor(model, data, "net21");
@@ -992,7 +992,7 @@ TEST_F(SensorTest, ContactNet) {
       mj_applyFT(model, data, force, torque, point, b2, qfrc.data());
 
       // compare
-      EXPECT_THAT(qfrc, Pointwise(MjNear(1e-6, 1e-4), qfrc_expected));
+      EXPECT_THAT(qfrc, Pointwise(MjNear(1e-6, 2e-4), qfrc_expected));
 
       nconmax = std::max(nconmax, data->ncon);
     }

test/engine/engine_solver_test.cc

@@ -64,9 +64,9 @@ TEST_F(SolverTest, IslandsEquivalent) {
   // 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(1e-1, 2),
-      MjTol(3e-2, 1),
-      MjTol(1.5e-4, 3.6)
+      MjTol(1e-1, 1.2e-1),
+      MjTol(3e-2, 2e-2),
+      MjTol(1.3e-5, 2.8e-3)
   };
 
   for (int i = 0; i < kNumTol; ++i) {
@@ -188,7 +188,7 @@ TEST_F(SolverTest, IslandsEquivalentForward) {
           int worst_idx = -1;
           mjtNum scale = 0.5 * (mju_norm(data_noisland->qacc, nv) +
                                 mju_norm(data_island->qacc, nv));
-          mjtNum rtol = solver == mjSOL_CG ? MjTol(1e-6, 1e-2)
+          mjtNum rtol = solver == mjSOL_CG ? MjTol(1e-8, 1e-4)
                                            : MjTol(1e-13, 1e-3);
           mjtNum worst_allowed = scale * rtol;
 
@@ -240,7 +240,7 @@ TEST_F(SolverTest, SolversEquivalent) {
        .tolerances =
            {
                .newton        = MjTol(1e-13, 1e-5),
-               .cg            = MjTol(1e-5,  1e-2),
+               .cg            = MjTol(1e-13, 1e-5),
                .pgs_pyramidal = MjTol(1e-12, 1e-5),
                .pgs_elliptic  = MjTol(1e-3,  1e-2),
            }},
@@ -248,7 +248,7 @@ TEST_F(SolverTest, SolversEquivalent) {
        .tolerances =
            {
                .newton        = MjTol(1e-13, 1e-5),
-               .cg            = MjTol(1e-5,  1e-2),
+               .cg            = MjTol(1e-12, 1e-5),
                .pgs_pyramidal = MjTol(1e-5,  1e-5),
                .pgs_elliptic  = MjTol(1e-8,  1e-4),
            }},