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

Author: yuvaltassa

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.
 

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);

python/mujoco/introspect/functions.py

@@ -3001,7 +3001,7 @@
                  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(

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);
   }
 }
 

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);

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<mjtNum> dst_sparse(nv * nv, 0.0);
+  // dense matrix, all values are 3.0
+  vector<mjtNum> dst_dense(nv * nv, 3.0);
 
-  // sparse zero matrix
-  vector<mjtNum> dst_dense(nv * nv, 0.0);
+  // sparse matrix, all values are 3.0
+  vector<mjtNum> dst_sparse(nv * nv, 3.0);
   vector<int> rownnz(nv, nv);
   vector<int> rowadr(nv, 0);
   vector<int> 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];