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
This makes mj_forward calls idempotent if warmstart is disabled and quats in qpos are normalized.
PiperOrigin-RevId: 564442666
Change-Id: I9c43add82bcfb4101c19fbfbfc8c30091b8906d5
Yuval Tassa