The primal cost has curvature of at least M in every zone, making it strongly
convex in the M-norm and bounding the suboptimality of any point by the
Fenchel duality gap at its constraint forces:
cost(qacc) - cost* <= 0.5*grad'*M^-1*grad
Since M's factorization always exists, this certificate is evaluable before
the solver does any work: one triangular solve and one dot product. When the
warmstarted solution is already certified to satisfy the tolerance, CG and
Newton now return with zero iterations; for Newton this skips building and
factorizing the Hessian. If the certificate declines, Newton gets a second
exit after factorization: the Newton decrement, checked before the first
line search.
Because the gap bounds cost suboptimality, stiff constraints can convert it
into force errors of order sqrt(2*gap*stiffness). Newton solutions are
characteristically force-accurate, so Newton zero-iteration exits also
require the gradient criterion, preserving constraint-force accuracy at
rest; CG solutions are characteristically cost-accurate and exit on the gap
alone.
On a settling pile of 50 boxes (300 dofs, ~200 contacts), end-to-end time
per step drops 13% over a settle-then-rest run and 27% in the quiescent
limit, with Newton iterations falling from 0.98 to 0.40 per step.
Tests: WarmstartZeroIterations sweeps solver/cone/jacobian on a settled box,
asserting zero iterations, forward/inverse consistency, and agreement with a
tolerance=0 control solve from the same state. WarmstartZeroIterationsIslands
checks per-island exits with a kicked box next to a settled one.
RefsiteConservesMomentum now requests an exact solve (tolerance=0), since it
asserts momentum conservation tighter than the solver tolerance contract.
PiperOrigin-RevId: 947993735
Change-Id: I2fd855774bff619709b2c386f1ba2714286e0821
After an accepted line-search step, the solver has already rebuilt the gradient
and Hessian and solved for the next search direction, so the Newton decrement
0.5*g'*H^-1*g -- the quadratic model's predicted cost improvement of the next
iteration -- costs one dot product. Terminating when it falls below tolerance
avoids running one more iteration only to observe a correspondingly small
actual improvement.
This is a C port of Alain's proposal in MJWarp:
https://github.com/google-deepmind/mujoco_warp/pull/1520
PiperOrigin-RevId: 947768034
Change-Id: I94e5c71a4e2b4a7775611edd1dad254bba2633b4
Reformulate the cost difference calculation (`ellipticCostDif`) to use mathematically equivalent formulas that avoid subtracting large, nearly equal values (cancellation errors) in single precision at high normal forces.
This is a C port of Alain's formulation in MJWarp:
https://github.com/google-deepmind/mujoco_warp/pull/1512
Also adds an integration test (`EllipticLineSearchPrecisionDiagnostics`) that reproduces the precision issue under large normal forces in the sliding regime, and asserts that the solver does not produce large negative improvements in either precision. This test failed before the change.
PiperOrigin-RevId: 946137815
Change-Id: Ia8fc1c4823b5fee770140c8989b9465737d22ad7
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