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
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
The introduction of site specification to connects and welds in 3.2.3 conditionally changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`. These changes were not properly propagated in several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and runtime enabling/disabling of such constraints.
PiperOrigin-RevId: 690670420
Change-Id: I55ee8a013cbee8457f8d6c7f33c2981aedafbab6
Yuval Tassa