From a230aa60c592e3c6fb05db7ec5790a059cea0719 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 23 Aug 2025 03:56:02 -0700 Subject: [PATCH] Improve mj_addM and associated test - Updated docs to clarify that only the lower triangle is touched - The function was unnecessarily adding the upper triangle in the sparse case - The test used an unnecessarily large model and stepping it leading to timeouts under ASAN PiperOrigin-RevId: 798528736 Change-Id: I592cc17dfb62379bb0cd6b332a8eb7e8b0ef6355 --- doc/APIreference/functions.rst | 2 +- include/mujoco/mujoco.h | 2 +- python/mujoco/introspect/functions.py | 2 +- src/engine/engine_support.c | 2 +- src/engine/engine_support.h | 2 +- test/engine/engine_support_test.cc | 32 +++++++++++++-------------- 6 files changed, 20 insertions(+), 22 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index c625ab2e..efd869a4 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -492,7 +492,7 @@ Multiply vector by (inertia matrix)^(1/2). .. mujoco-include:: mj_addM -Add inertia matrix to destination matrix. +Add inertia matrix to destination matrix (lower triangle only). Destination can be sparse or dense when all int* are NULL. diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 677d280a..c16556e5 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -512,7 +512,7 @@ MJAPI void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* // Multiply vector by (inertia matrix)^(1/2). MJAPI void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); -// Add inertia matrix to destination matrix. +// Add inertia matrix to destination matrix (lower triangle only). // Destination can be sparse or dense when all int* are NULL. // Nullable: rownnz, rowadr, colind MJAPI void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int* rownnz, int* rowadr, int* colind); diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index e925870b..65d8e05f 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -3001,7 +3001,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ nullable=True, ), ), - doc='Add inertia matrix to destination matrix. Destination can be sparse or dense when all int* are NULL.', # pylint: disable=line-too-long + doc='Add inertia matrix to destination matrix (lower triangle only). Destination can be sparse or dense when all int* are NULL.', # pylint: disable=line-too-long )), ('mj_applyFT', FunctionDecl( diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index e0952b3d..a68d6a22 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -1038,7 +1038,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, // dense else { - mju_addToSymSparse(dst, d->M, nv, m->M_rownnz, m->M_rowadr, m->M_colind, /*flg_upper=*/ 1); + mju_addToSymSparse(dst, d->M, nv, m->M_rownnz, m->M_rowadr, m->M_colind, /*flg_upper*/ 0); } } diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index 9382d58c..b28da6f7 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -129,7 +129,7 @@ MJAPI void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* // multiply vector by (inertia matrix)^(1/2) MJAPI void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); -// add inertia matrix to destination matrix +// add inertia matrix to destination matrix (lower triangle only) // destination can be sparse or dense when all int* are NULL MJAPI void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int* rownnz, int* rowadr, int* colind); diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index ff5b13ff..141b7e32 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -743,24 +743,24 @@ TEST_F(SupportTest, GetSetStateStepEqual) { using InertiaTest = MujocoTest; -TEST_F(InertiaTest, DenseSameAsSparse) { - mjModel* m = LoadModelFromPath("humanoid/humanoid100.xml"); - mjData* d = mj_makeData(m); +static const char* const kInertiaPath = "engine/testdata/inertia.xml"; + +TEST_F(InertiaTest, AddMdenseSameAsSparse) { + const std::string xml_path = GetTestDataFilePath(kInertiaPath); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; int nv = m->nv; - // force use of sparse matrices - m->opt.jacobian = mjJAC_SPARSE; + mjData* d = mj_makeData(m); - // warm-up rollout to get a typical state - while (d->time < 2) { - mj_step(m, d); - } + mj_step(m, d); - // dense zero matrix - vector dst_sparse(nv * nv, 0.0); + // dense matrix, all values are 3.0 + vector dst_dense(nv * nv, 3.0); - // sparse zero matrix - vector dst_dense(nv * nv, 0.0); + // sparse matrix, all values are 3.0 + vector dst_sparse(nv * nv, 3.0); vector rownnz(nv, nv); vector rowadr(nv, 0); vector colind(nv * nv, 0); @@ -780,9 +780,9 @@ TEST_F(InertiaTest, DenseSameAsSparse) { // dense addM mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr); - // dense comparison, lower triangle should match + // dense comparison (lower triangle) for (int i=0; i < nv; i++) { - for (int j=0; j < i; j++) { + for (int j=0; j < nv; j++) { EXPECT_EQ(dst_dense[i*nv+j], dst_sparse[i*nv+j]); } } @@ -792,8 +792,6 @@ TEST_F(InertiaTest, DenseSameAsSparse) { mj_deleteModel(m); } -static const char* const kInertiaPath = "engine/testdata/inertia.xml"; - TEST_F(InertiaTest, mulM) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024];