diff --git a/doc/changelog.rst b/doc/changelog.rst index ede272c8..798cfde7 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -153,6 +153,8 @@ Bug fixes 26. Fixed a bug that was causing :ref:`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) ----------------------------- diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index cc1011b7..b4335e05 100644 --- a/src/engine/engine_core_constraint.c +++ b/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: