Fix setting wrong value in efc_diagApprox for rotational components of weld constraints.

PiperOrigin-RevId: 572276016
Change-Id: Ic985a342777aca976f5c4bc781133fb4a8976e3f

doc/changelog.rst

@@ -153,6 +153,8 @@ Bug fixes
 26. Fixed a bug that was causing :ref:`geom margin<body-geom-margin>` to be ignored during the construction of
     midphase collision trees.
 
+27. Fixed a bug that was generating incorrect values in ``efc_diagApprox`` for weld equality constraints.
+
 Version 2.3.7 (July 20, 2023)
 -----------------------------
 

src/engine/engine_core_constraint.c

@@ -1035,11 +1035,6 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
     // get constraint id
     id = d->efc_id[i];
 
-    // clear weld counter
-    if (d->efc_type[i] != mjEQ_WELD) {
-      weldcnt = 0;
-    }
-
     // process according to constraint type
     switch ((mjtConstraint) d->efc_type[i]) {
     case mjCNSTR_EQUALITY:
@@ -1052,13 +1047,13 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
         dA[i] = m->body_invweight0[2*b1] + m->body_invweight0[2*b2];
         break;
 
-      case mjEQ_WELD:  // distingush translation and rotation inertia
+      case mjEQ_WELD:  // distinguish translation and rotation inertia
         // body translation or rotation depending on weldcnt
         b1 = m->eq_obj1id[id];
         b2 = m->eq_obj2id[id];
         dA[i] = m->body_invweight0[2*b1 + (weldcnt > 2)] +
                 m->body_invweight0[2*b2 + (weldcnt > 2)];
-        weldcnt++;
+        weldcnt = (weldcnt + 1) % 6;
         break;
 
       case mjEQ_JOINT: