diff --git a/doc/changelog.rst b/doc/changelog.rst index 0da93da0..2eaddeb0 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -28,6 +28,10 @@ General (multi-threaded). The island-specific matrices ``iM, iLD, iefc_J`` were removed from the arena and are now allocated on the stack. + - Following the introduction of the :ref:`diagexact` flag, the ``mjData`` field + ``efc_diagApprox`` was renamed to ``efc_diagA``, as it can now be either the exact or approximate diagonal of + the :math:`A` ("Delassus") matrix. + Bug fixes ^^^^^^^^^ - Fixed a bug in the ``mjz`` :ref:`decoder ` where unnormalized paths would fail to be read. diff --git a/doc/includes/references.h b/doc/includes/references.h index 03f478c5..32828541 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -318,7 +318,7 @@ struct mjData_ { mjtNum* efc_pos; // constraint position (equality, contact) (nefc x 1) mjtNum* efc_margin; // inclusion margin (contact) (nefc x 1) mjtNum* efc_frictionloss; // frictionloss (friction) (nefc x 1) - mjtNum* efc_diagApprox; // approximation to diagonal of A (nefc x 1) + mjtNum* efc_diagA; // diagonal of A matrix, approximate or exact (nefc x 1) mjtNum* efc_KBIP; // stiffness, damping, impedance, imp' (nefc x 4) mjtNum* efc_D; // constraint mass (nefc x 1) mjtNum* efc_R; // inverse constraint mass (nefc x 1) diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index aab0709a..e0d796af 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -341,7 +341,7 @@ struct mjData_ { mjtNum* efc_pos; // constraint position (equality, contact) (nefc x 1) mjtNum* efc_margin; // inclusion margin (contact) (nefc x 1) mjtNum* efc_frictionloss; // frictionloss (friction) (nefc x 1) - mjtNum* efc_diagApprox; // approximation to diagonal of A (nefc x 1) + mjtNum* efc_diagA; // diagonal of A matrix, approximate or exact (nefc x 1) mjtNum* efc_KBIP; // stiffness, damping, impedance, imp' (nefc x 4) mjtNum* efc_D; // constraint mass (nefc x 1) mjtNum* efc_R; // inverse constraint mass (nefc x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index ec3e0da3..3bff5c53 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -933,7 +933,7 @@ X ( mjtNum, efc_pos, MJ_D(nefc), 1 ) \ X ( mjtNum, efc_margin, MJ_D(nefc), 1 ) \ X ( mjtNum, efc_frictionloss, MJ_D(nefc), 1 ) \ - X ( mjtNum, efc_diagApprox, MJ_D(nefc), 1 ) \ + X ( mjtNum, efc_diagA, MJ_D(nefc), 1 ) \ X ( mjtNum, efc_KBIP, MJ_D(nefc), 4 ) \ X ( mjtNum, efc_D, MJ_D(nefc), 1 ) \ X ( mjtNum, efc_R, MJ_D(nefc), 1 ) \ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 78f97fd0..84376c21 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -6406,11 +6406,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ array_extent=('nefc',), ), StructFieldDecl( - name='efc_diagApprox', + name='efc_diagA', type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='approximation to diagonal of A', + doc='diagonal of A matrix, approximate or exact', array_extent=('nefc',), ), StructFieldDecl( diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index e7a521a0..52c7feae 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -1651,7 +1651,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) { void mj_diagApprox(const mjModel* m, mjData* d) { int id, dim, b1, b2, f, weldcnt = 0; int nefc = d->nefc; - mjtNum tran, rot, fri, *dA = d->efc_diagApprox; + mjtNum tran, rot, fri, *dA = d->efc_diagA; mjContact* con = NULL; // loop over all constraints, compute approximate inverse inertia @@ -2079,7 +2079,7 @@ static void getimpedance(const mjtNum* solimp, mjtNum pos, mjtNum margin, } -// compute efc_R, efc_D, efc_KBIP, adjust efc_diagApprox +// compute efc_R, efc_D, efc_KBIP, adjust efc_diagA void mj_makeImpedance(const mjModel* m, mjData* d) { int dim, nefc = d->nefc; mjtNum *R = d->efc_R, *KBIP = d->efc_KBIP; @@ -2099,7 +2099,7 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { // set R and KBIP for all constraint dimensions for (int j=0; j < dim; j++) { // R = (1-imp)/imp * diagApprox - R[i+j] = mju_max(mjMINVAL, (1-imp)*d->efc_diagApprox[i+j]/imp); + R[i+j] = mju_max(mjMINVAL, (1-imp)*d->efc_diagA[i+j]/imp); // constraint type int tp = d->efc_type[i+j]; @@ -2190,9 +2190,9 @@ void mj_makeImpedance(const mjModel* m, mjData* d) { d->efc_D[i] = 1 / R[i]; } - // adjust diagApprox so that R = (1-imp)/imp * diagApprox + // adjust diagA so that R = (1-imp)/imp * diagA for (int i=0; i < nefc; i++) { - d->efc_diagApprox[i] = R[i] * KBIP[4*i+2] / (1-KBIP[4*i+2]); + d->efc_diagA[i] = R[i] * KBIP[4*i+2] / (1-KBIP[4*i+2]); } } @@ -2830,12 +2830,12 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { // compute diagApprox mj_diagApprox(m, d); - // compute KBIP, D, R, adjust diagApprox + // compute KBIP, D, R, adjust diagA mj_makeImpedance(m, d); } -// compute Y = J*M^{-1/2}; if flg_diagexact, overwrite efc_diagApprox with ||Y_i||^2 +// compute Y = J*M^{-1/2}; if flg_diagexact, overwrite efc_diagA with ||Y_i||^2 static void mj_makeY(const mjModel* m, mjData* d, int flg_diagexact) { int nefc = d->nefc, nv = m->nv; @@ -2888,12 +2888,12 @@ static void mj_makeY(const mjModel* m, mjData* d, int flg_diagexact) { d->efc_Y_colind, nefc, d->qLD, m->M_rownnz, m->M_rowadr, m->M_colind, sqrtInvD); - // overwrite diagApprox with exact diagonal: diagApprox[i] = ||Y_i||^2 + // overwrite diagA with exact diagonal: diagA[i] = ||Y_i||^2 if (flg_diagexact) { for (int i=0; i < nefc; i++) { int adr = d->efc_Y_rowadr[i]; int nnz = d->efc_Y_rownnz[i]; - d->efc_diagApprox[i] = mju_dot(d->efc_Y+adr, d->efc_Y+adr, nnz); + d->efc_diagA[i] = mju_dot(d->efc_Y+adr, d->efc_Y+adr, nnz); } } } @@ -2914,10 +2914,10 @@ static void mj_makeY(const mjModel* m, mjData* d, int flg_diagexact) { // Y = backsubM2(J')' mj_solveM2(m, d, d->efc_Y, d->efc_J, sqrtInvD, nefc); - // overwrite diagApprox with exact diagonal: diagApprox[i] = ||Y_i||^2 + // overwrite diagA with exact diagonal: diagA[i] = ||Y_i||^2 if (flg_diagexact) { for (int i=0; i < nefc; i++) { - d->efc_diagApprox[i] = mju_dot(d->efc_Y+i*nv, d->efc_Y+i*nv, nv); + d->efc_diagA[i] = mju_dot(d->efc_Y+i*nv, d->efc_Y+i*nv, nv); } } } @@ -3023,7 +3023,7 @@ static void mj_makeAR(const mjModel* m, mjData* d) { } -// compute efc_Y, optionally efc_diagApprox, optionally efc_AR +// compute efc_Y, optionally efc_diagA, optionally efc_AR void mj_projectConstraint(const mjModel* m, mjData* d) { int nefc = d->nefc; diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index 9d3f9eb0..484a30ec 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -74,10 +74,10 @@ int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int di //------------------------ parameter computation/extraction ---------------------------------------- -// compute efc_diagApprox +// compute efc_diagA void mj_diagApprox(const mjModel* m, mjData* d); -// compute efc_R, efc_D, efc_KDIP, adjust diagApprox +// compute efc_R, efc_D, efc_KDIP, adjust diagA void mj_makeImpedance(const mjModel* m, mjData* d); diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index f6ca332b..b5c92ffe 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -1598,7 +1598,7 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena printArray2d("EFC_POS", d->nefc, 1, d->efc_pos, fp, float_format); printArray2d("EFC_MARGIN", d->nefc, 1, d->efc_margin, fp, float_format); printArray2d("EFC_FRICTIONLOSS", d->nefc, 1, d->efc_frictionloss, fp, float_format); - printArray2d("EFC_DIAGAPPROX", d->nefc, 1, d->efc_diagApprox, fp, float_format); + printArray2d("EFC_DIAGA", d->nefc, 1, d->efc_diagA, fp, float_format); printArray2d("EFC_KBIP", d->nefc, 4, d->efc_KBIP, fp, float_format); printArray2d("EFC_D", d->nefc, 1, d->efc_D, fp, float_format); printArray2d("EFC_R", d->nefc, 1, d->efc_R, fp, float_format); diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 8cffb5e0..5cd69696 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -146,7 +146,7 @@ TEST_F(CoreConstraintTest, EqualityBodySite) { ASSERT_GT(data->time, time) << "Divergence detected"; } int nefc_site = data->nefc; - std::vector dA = AsVector(data->efc_diagApprox, nefc_site); + std::vector dA = AsVector(data->efc_diagA, nefc_site); // reset mj_resetData(model, data); @@ -163,7 +163,7 @@ TEST_F(CoreConstraintTest, EqualityBodySite) { // compare EXPECT_EQ(nefc_site, data->nefc); - EXPECT_THAT(AsVector(data->efc_diagApprox, data->nefc), + EXPECT_THAT(AsVector(data->efc_diagA, data->nefc), Pointwise(MjNear(1e-12, 1e-4), dA)); mj_deleteData(data); diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 47bae7c8..3342bbd8 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -440,7 +440,7 @@ TEST_F(UserFlexTest, TrilinearInterpolation) { EXPECT_EQ(d2->nefc, 4*(d2->contact[0].dim-1)*2); EXPECT_EQ(d1->nJ, d2->nJ); for (int i = 0; i < d1->nefc; ++i) { - EXPECT_EQ(d1->efc_diagApprox[i], d2->efc_diagApprox[i]); + EXPECT_EQ(d1->efc_diagA[i], d2->efc_diagA[i]); EXPECT_EQ(d1->efc_D[i], d2->efc_D[i]); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index e9baa2ea..69877044 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5735,7 +5735,7 @@ public unsafe struct mjData_ { public double* efc_pos; public double* efc_margin; public double* efc_frictionloss; - public double* efc_diagApprox; + public double* efc_diagA; public double* efc_KBIP; public double* efc_D; public double* efc_R; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index eae8ae44..36cc5dfa 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -6930,8 +6930,8 @@ struct MjData { emscripten::val efc_frictionloss() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nefc, ptr_->efc_frictionloss)); } - emscripten::val efc_diagApprox() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nefc, ptr_->efc_diagApprox)); + emscripten::val efc_diagA() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nefc, ptr_->efc_diagA)); } emscripten::val efc_KBIP() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nefc * 4, ptr_->efc_KBIP)); @@ -11635,7 +11635,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("efc_Y_rownnz", &MjData::efc_Y_rownnz) .property("efc_aref", &MjData::efc_aref) .property("efc_b", &MjData::efc_b) - .property("efc_diagApprox", &MjData::efc_diagApprox) + .property("efc_diagA", &MjData::efc_diagA) .property("efc_force", &MjData::efc_force) .property("efc_frictionloss", &MjData::efc_frictionloss) .property("efc_id", &MjData::efc_id) diff --git a/wasm/codegen/generators/constants.py b/wasm/codegen/generators/constants.py index 086746b3..9a20444d 100644 --- a/wasm/codegen/generators/constants.py +++ b/wasm/codegen/generators/constants.py @@ -320,7 +320,7 @@ MJDATA_SIZES: tuple[str, ...] = ( "efc_R", "efc_aref", "efc_b", - "efc_diagApprox", + "efc_diagA", "efc_force", "efc_frictionloss", "efc_id",