From 1bd6c94e6d588561683c32bd24de98ca16af0c0c Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Tue, 10 Oct 2023 09:33:42 -0700 Subject: [PATCH] Fix setting wrong value in efc_diagApprox for rotational components of weld constraints. PiperOrigin-RevId: 572276016 Change-Id: Ic985a342777aca976f5c4bc781133fb4a8976e3f --- doc/changelog.rst | 2 ++ src/engine/engine_core_constraint.c | 9 ++------- 2 files changed, 4 insertions(+), 7 deletions(-) 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: