The gyroscopic (bias) derivatives applied to standalone free bodies by the
implicitfast integrator provide comparable stability for spinning bodies,
with none of midpoint's restrictions: they apply under contacts, fluid
forces and constraints, and preserve the linear force-velocity relation
required by discrete-time inverse dynamics. The invdiscrete flag reverts to
its original single meaning and no longer affects forward dynamics.
Restore implicitfast coverage in the DiscreteInverseMatch test, removed
when midpoint made discrete inverse dynamics untestable.
Add implicit gyroscopic (bias) derivatives for free bodies in implicitfast.
The implicitfast integrator drops the RNE (bias) derivative to stay on the
symmetric Cholesky path, so fast-spinning free bodies integrate gyroscopic
forces explicitly and can gain energy. Symmetrizing the gyroscopic Jacobian
is not an option: its stabilizing content is the antisymmetric part, and
adding only the symmetric part is destabilizing.
Instead, exploit the fact that for a standalone free body the 6x6 block of
M - h*D is decoupled from the rest of the system (qDeriv sparsity is
tree-local): after the global solve, rebuild the block with the exact bias
derivative in closed form (mjd_freeBias_vel) and re-solve it with dense
unsymmetric LU, overwriting the block's rows of qacc. For lone spinning
bodies this makes implicitfast match implicit to rounding, at ~150ns per
eligible body: cheaper than the midpoint machinery it will replace.
Eligibility is structural only; contacts, fluid and constraints need no
gating. The same block is mirrored in discrete inverse dynamics
(mj_discreteAcc), making invdiscrete exact for spinning free bodies.
PiperOrigin-RevId: 948472495
Change-Id: I813ef3d98c7b399881bc8603b9f9208cfb02eb58
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