Refactor mj_fullM. This change is part of the deprecation of mjData.qM.

PiperOrigin-RevId: 925669464
Change-Id: I4889c66591bc1df4c31135a13776052aad491f7a

doc/APIreference/functions.rst

@@ -601,14 +601,7 @@ 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.
+Convert sparse inertia matrix into full (i.e. dense) matrix.
-|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:
 

doc/APIreference/functions_override.rst

@@ -342,17 +342,6 @@ found, the function will return ``distmax`` and ``fromto``, if given, will be se
    As explained in :ref:`Collision Detection<coDistance>`, distances are inaccurate when using the
    :ref:`legacy CCD pipeline<coCCD>`, and its use is discouraged.
 
-.. _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 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:
 
 This function multiplies the joint-space inertia matrix stored in ``mjData.M`` by a vector.

doc/changelog.rst

@@ -30,6 +30,12 @@ General
        the :math:`A` ("Delassus") matrix.
      - The deprecated functions ``mju_{error,warning}_{i,s}`` have been removed.
 
+     - Changed the signature of :ref:`mj_fullM` from ``mj_fullM(m, dst, M)`` to ``mj_fullM(m, d, dst)`` as part of the
+       planned deprecation of ``mjData.qM`` in favor of the CSR-format ``mjData.M``.
+
+       **Migration:** For inertia matrix conversion, replace ``mj_fullM(m, dst, d->qM)`` with ``mj_fullM(m, d, dst)`` or
+       ``mju_sym2dense(dst, d->M, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind)``.
+
 Bug fixes
 ^^^^^^^^^
 - Fixed a bug in the ``mjz`` :ref:`decoder <mjpDecoder>` where unnormalized paths would fail to be read.

doc/includes/references.h

@@ -3320,7 +3320,7 @@ void mj_jacDot(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr,
 void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body);
 int mj_name2id(const mjModel* m, int type, const char* name);
 const char* mj_id2name(const mjModel* m, int type, int id);
-void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M);
+void mj_fullM(const mjModel* m, const mjData* d, mjtNum* dst);
 void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
 void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);
 void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int* rownnz, int* rowadr, int* colind);

include/mujoco/mujoco.h

@@ -598,8 +598,8 @@ MJAPI int mj_name2id(const mjModel* m, int type, const char* name);
 // Get name of object with the specified mjtObj type and id; return NULL if name not found.
 MJAPI const char* mj_id2name(const mjModel* m, int type, int id);
 
-// Convert sparse inertia matrix M into full (i.e. dense) matrix.
+// Convert sparse inertia matrix into full (i.e. dense) matrix.
-MJAPI void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M);
+MJAPI void mj_fullM(const mjModel* m, const mjData* d, mjtNum* dst);
 
 // Multiply vector by inertia matrix.
 MJAPI void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);

python/mujoco/functions.cc

@@ -591,15 +591,12 @@ PYBIND11_MODULE(_functions, pymodule) {
   Def<traits::mj_id2name>(pymodule);
   Def<traits::mj_fullM>(
       pymodule,
-      [](const raw::MjModel* m, Eigen::Ref<EigenArrayXX> dst,
+      [](const raw::MjModel* m, const raw::MjData* d,
-         Eigen::Ref<const EigenVectorX> M) {
+         Eigen::Ref<EigenArrayXX> dst) {
-        if (M.size() != m->nM) {
-          throw py::type_error("M should be of size nM");
-        }
         if (dst.cols() != m->nv || dst.rows() != m->nv) {
           throw py::type_error("dst should be of shape (nv, nv)");
         }
-        return ::mj_fullM(m, dst.data(), M.data());
+        return ::mj_fullM(m, d, dst.data());
       });
   Def<traits::mj_mulM>(
       pymodule,

python/mujoco/introspect/functions.py

@@ -3478,20 +3478,20 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
                      inner_type=ValueType(name='mjModel', is_const=True),
                  ),
              ),
+             FunctionParameterDecl(
+                 name='d',
+                 type=PointerType(
+                     inner_type=ValueType(name='mjData', is_const=True),
+                 ),
+             ),
              FunctionParameterDecl(
                  name='dst',
                  type=PointerType(
                      inner_type=ValueType(name='mjtNum'),
                  ),
              ),
-             FunctionParameterDecl(
-                 name='M',
-                 type=PointerType(
-                     inner_type=ValueType(name='mjtNum', is_const=True),
-                 ),
-             ),
          ),
-         doc='Convert sparse inertia matrix M into full (i.e. dense) matrix.',
+         doc='Convert sparse inertia matrix into full (i.e. dense) matrix.',
      )),
     ('mj_mulM',
      FunctionDecl(

src/engine/engine_support.c

@@ -367,19 +367,8 @@ void mj_setKeyframe(mjModel* m, const mjData* d, int k) {
 //-------------------------- inertia functions -----------------------------------------------------
 
 // convert sparse inertia matrix M into full matrix
-void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M) {
+void mj_fullM(const mjModel* m, const mjData* d, mjtNum* dst) {
-  int adr = 0, nv = m->nv;
+  mju_sym2dense(dst, d->M, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind);
-  mju_zero(dst, nv*nv);
-
-  for (int i=0; i < nv; i++) {
-    int j = i;
-    while (j >= 0) {
-      dst[i*nv+j] = M[adr];
-      dst[j*nv+i] = M[adr];
-      j = m->dof_parentid[j];
-      adr++;
-    }
-  }
 }
 
 

src/engine/engine_support.h

@@ -58,7 +58,7 @@ MJAPI void mj_setKeyframe(mjModel* m, const mjData* d, int k);
 //-------------------------- inertia functions -----------------------------------------------------
 
 // convert sparse inertia matrix M into full matrix
-MJAPI void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M);
+MJAPI void mj_fullM(const mjModel* m, const mjData* d, mjtNum* dst);
 
 // multiply vector by inertia matrix
 MJAPI void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec);

test/engine/engine_core_smooth_test.cc

@@ -233,13 +233,13 @@ TEST_F(CoreSmoothTest, TendonArmature) {
 
     // get full M, includes both CRB and tendon inertia
     vector<mjtNum> M(nv*nv);
-    mj_fullM(m, M.data(), d->qM);
+    mj_fullM(m, d, M.data());
 
     // put only CRB inertia in M2
     mj_crb(m, d);
     mju_scatter(d->qM, d->M, m->mapM2M, m->nC);
     vector<mjtNum> M2(nv*nv);
-    mj_fullM(m, M2.data(), d->qM);
+    mj_fullM(m, d, M2.data());
 
     vector<mjtNum> ten_J(nv);     // tendon Jacobian
     vector<mjtNum> ten_M(nv*nv);  // tendon inertia
@@ -681,7 +681,7 @@ TEST_F(CoreSmoothTest, FactorI) {
 
   // dense M matrix
   vector<mjtNum> Mexpected(nv*nv);
-  mj_fullM(model, Mexpected.data(), data->qM);
+  mj_fullM(model, data, Mexpected.data());
 
   // expect matrices to match to floating point precision
   EXPECT_THAT(M, Pointwise(MjNear(1e-12, 1e-5), Mexpected));
@@ -690,6 +690,21 @@ TEST_F(CoreSmoothTest, FactorI) {
   mj_deleteModel(model);
 }
 
+// Convert legacy-format symmetric matrix to dense (local helper for tests).
+static void legacyToDense(const mjModel* m, mjtNum* dst, const mjtNum* M) {
+  int adr = 0, nv = m->nv;
+  mju_zero(dst, nv*nv);
+  for (int i = 0; i < nv; i++) {
+    int j = i;
+    while (j >= 0) {
+      dst[i*nv+j] = M[adr];
+      dst[j*nv+i] = M[adr];
+      j = m->dof_parentid[j];
+      adr++;
+    }
+  }
+}
+
 TEST_F(CoreSmoothTest, SolveLDs) {
   const std::string xml_path = GetTestDataFilePath(kInertiaPath);
   char error[1024];
@@ -712,7 +727,7 @@ TEST_F(CoreSmoothTest, SolveLDs) {
   mju_sparse2dense(LDdense.data(), d->qLD, nv, nv,
                    m->M_rownnz, m->M_rowadr, m->M_colind);
   vector<mjtNum> LDdense2(nv*nv);
-  mj_fullM(m, LDdense2.data(), LDlegacy.data());
+  legacyToDense(m, LDdense2.data(), LDlegacy.data());
 
   // expect lower triangles to match exactly
   for (int i=0; i < nv; i++) {

test/engine/engine_derivative_test.cc

@@ -848,7 +848,7 @@ TEST_F(DerivativeTest, LinearSystemInverse) {
 
   // expect that acceleration derivatives are the mass matrix
   vector<mjtNum> DfDa_expect(nv*nv, 0);
-  mj_fullM(model, DfDa_expect.data(), data->qM);
+  mj_fullM(model, data, DfDa_expect.data());
   EXPECT_THAT(DfDa, Pointwise(DoubleNear(eps), DfDa_expect));
 
   // expect that sensor derivatives w.r.t position only see sensor 1 at dof 0

test/engine/engine_support_test.cc

@@ -608,13 +608,13 @@ TEST_F(InertiaTest, FullM) {
   ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
   int nv = m->nv;
 
-  // forward dynamics, populate qM and qLD
+  // forward dynamics, populate M and qLD
   mjData* d = mj_makeData(m);
   mj_forward(m, d);
 
-  // get dense mass matrix from M using mju_sym2dense
+  // get dense mass matrix from M using mj_fullM
   vector<mjtNum> M(nv * nv);
-  mju_sym2dense(M.data(), d->M, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
+  mj_fullM(m, d, M.data());
 
   // get dense mass matrix from M using mju_sparse2dense
   vector<mjtNum> M_CSR(nv * nv);

unity/Runtime/Bindings/MjBindings.cs

@@ -6946,7 +6946,7 @@ public static unsafe extern int mj_name2id(mjModel_* m, int type, [MarshalAs(Unm
 public static unsafe extern IntPtr mj_id2name(mjModel_* m, int type, int id);
 
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
-public static unsafe extern void mj_fullM(mjModel_* m, double* dst, double* M);
+public static unsafe extern void mj_fullM(mjModel_* m, mjData_* d, double* dst);
 
 [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
 public static unsafe extern void mj_mulM(mjModel_* m, mjData_* d, double* res, double* vec);

wasm/codegen/generated/bindings.cc

@@ -8709,12 +8709,10 @@ void mj_forwardSkip_wrapper(const MjModel& m, MjData& d, int skipstage, int skip
   mj_forwardSkip(m.get(), d.get(), skipstage, skipsensor);
 }
 
-void mj_fullM_wrapper(const MjModel& m, const val& dst, const NumberArray& M) {
+void mj_fullM_wrapper(const MjModel& m, const MjData& d, const val& dst) {
   UNPACK_VALUE(mjtNum, dst);
-  UNPACK_ARRAY(mjtNum, M);
-  CHECK_SIZE(M, m.nM());
   CHECK_SIZE(dst, m.nv() * m.nv());
-  mj_fullM(m.get(), dst_.data(), M_.data());
+  mj_fullM(m.get(), d.get(), dst_.data());
 }
 
 void mj_fwdAcceleration_wrapper(const MjModel& m, MjData& d) {

wasm/codegen/generators/constants.py

@@ -565,7 +565,6 @@ FUNCTION_BOUNDS_CHECKS: Dict[str, str] = {
   CHECK_SIZE(qpos2, m.nq());
     """.strip(),
     "mj_fullM": """
-  CHECK_SIZE(M, m.nM());
   CHECK_SIZE(dst, m.nv() * m.nv());
     """.strip(),
     "mj_geomDistance": """