PUBLIC: In mju_normalize4, don't normalize if norm is less than mjMINVAL away from 1.

This makes mj_forward calls idempotent if warmstart is disabled and quats in qpos are normalized.

PiperOrigin-RevId: 564442666
Change-Id: I9c43add82bcfb4101c19fbfbfc8c30091b8906d5

src/engine/engine_util_blas.c

@@ -220,7 +220,7 @@ mjtNum mju_normalize4(mjtNum vec[4]) {
     vec[1] = 0;
     vec[2] = 0;
     vec[3] = 0;
-  } else {
+  } else if (mju_abs(norm - 1) > mjMINVAL) {
     mjtNum normInv = 1/norm;
     vec[0] *= normInv;
     vec[1] *= normInv;