From 972ffd7b90b0d1e4bd077b20002a95746e213b8e Mon Sep 17 00:00:00 2001
From: Yuval Tassa <tassa@google.com>
Date: Sun, 17 Aug 2025 11:01:18 -0700
Subject: [PATCH] Improve `mj_fullM` documentation, add clarifying test.

PiperOrigin-RevId: 796134559
Change-Id: I85ab3680d3545ccf83d0d3af8a7c21cb36e96d4e
---
 doc/APIreference/functions.rst          |  8 +++-
 doc/APIreference/functions_override.rst |  8 +++-
 test/engine/engine_support_test.cc      | 55 +++++++++++++++++++++++++
 3 files changed, 69 insertions(+), 2 deletions(-)

diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst
index b7d21cfd..e0ad2b0c 100644
--- a/doc/APIreference/functions.rst
+++ b/doc/APIreference/functions.rst
@@ -459,7 +459,13 @@ Get name of object with the specified :ref:`mjtObj` type and id, returns ``NULL`
 .. mujoco-include:: mj_fullM
 
 Convert sparse inertia matrix ``M`` into full (i.e. dense) matrix.
-|br| ``dst`` must be of size ``nv x nv``, ``M`` must be of the same size as ``mjData.qM``.
+|br| ``dst`` must be of size ``nv x nv``, ``M`` must be of the same structure as ``mjData.qM``.
+
+The ``mjData`` members ``qM`` and ``M`` represent the same matrix in different formats; the former is unique to
+MuJoCo, the latter is standard Compressed Sparse Row (lower triangle only). The :math:`L^T D L` factor of the inertia
+matrix ``mjData.qLD`` uses the same CSR format as ``mjData.M``. See
+`engine_support_test <https://github.com/google-deepmind/mujoco/blob/main/test/engine/engine_support_test.cc>`__ for
+pedagogical examples.
 
 .. _mj_mulM:
 
diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst
index 927a4f43..c0556d26 100644
--- a/doc/APIreference/functions_override.rst
+++ b/doc/APIreference/functions_override.rst
@@ -266,7 +266,13 @@ found, the function will return ``distmax`` and ``fromto``, if given, will be se
 .. _mj_fullM:
 
 Convert sparse inertia matrix ``M`` into full (i.e. dense) matrix.
-|br| ``dst`` must be of size ``nv x nv``, ``M`` must be of the same size as ``mjData.qM``.
+|br| ``dst`` must be of size ``nv x nv``, ``M`` must be of the same structure as ``mjData.qM``.
+
+The ``mjData`` members ``qM`` and ``M`` represent the same matrix in different formats; the former is unique to
+MuJoCo, the latter is standard Compressed Sparse Row (lower triangle only). The :math:`L^T D L` factor of the inertia
+matrix ``mjData.qLD`` uses the same CSR format as ``mjData.M``. See
+`engine_support_test <https://github.com/google-deepmind/mujoco/blob/main/test/engine/engine_support_test.cc>`__ for
+pedagogical examples.
 
 .. _mj_mulM:
 
diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc
index ec97e704..ff5b13ff 100644
--- a/test/engine/engine_support_test.cc
+++ b/test/engine/engine_support_test.cc
@@ -858,6 +858,61 @@ TEST_F(InertiaTest, mulM2) {
   mj_deleteModel(model);
 }
 
+TEST_F(InertiaTest, FullM) {
+  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;
+
+  // forward dynamics, populate qM and qLD
+  mjData* d = mj_makeData(m);
+  mj_forward(m, d);
+
+  // get dense mass matrix from qM using mj_fullM
+  vector<mjtNum> M(nv * nv);
+  mj_fullM(m, M.data(), d->qM);
+
+  // get dense mass matrix from M using mju_sparse2dense
+  vector<mjtNum> M_CSR(nv * nv);
+  mju_sparse2dense(M_CSR.data(), d->M, nv, nv,
+                   m->M_rownnz, m->M_rowadr, m->M_colind);
+
+  // expect lower triangles to match exactly
+  for (int i = 0; i < nv; ++i) {
+    for (int j = 0; j <= i; ++j) {
+      EXPECT_EQ(M[i * nv + j], M_CSR[i * nv + j]);
+    }
+  }
+
+  // get dense LTDL factor (D on the diagonal)
+  vector<mjtNum> LD(nv * nv);
+  mju_sparse2dense(LD.data(), d->qLD, nv, nv,
+                   m->M_rownnz, m->M_rowadr, m->M_colind);
+
+  // extract L and D from LD
+  vector<mjtNum> L = LD;
+  vector<mjtNum> D(nv * nv, 0.0);
+  for (int i = 0; i < nv; i++) {
+    D[i * nv + i] = LD[i * nv + i];
+    L[i * nv + i] = 1.0;
+  }
+
+  // compute DL = D * L
+  vector<mjtNum> DL(nv * nv, 0.0);
+  mju_mulMatMat(DL.data(), D.data(), L.data(), nv, nv, nv);
+
+  // compute the triple product P = L^T * D * L
+  vector<mjtNum> P(nv * nv, 0.0);
+  mju_mulMatTMat(P.data(), L.data(), DL.data(), nv, nv, nv);
+
+  // expect M and P to match to high precision
+  EXPECT_THAT(M, Pointwise(DoubleNear(1e-10), P));
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
 static constexpr char GeomDistanceTestingModel[] = R"(
 <mujoco>
   <option>