diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 10dd03da..0a9e1dd7 100644 --- a/doc/APIreference/functions.rst +++ b/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. -|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 `__ for -pedagogical examples. +Convert sparse inertia matrix into full (i.e. dense) matrix. .. _mj_mulM: diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index 88e90085..1ca383fe 100644 --- a/doc/APIreference/functions_override.rst +++ b/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`, distances are inaccurate when using the :ref:`legacy CCD pipeline`, 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 `__ for -pedagogical examples. - .. _mj_mulM: This function multiplies the joint-space inertia matrix stored in ``mjData.M`` by a vector. diff --git a/doc/changelog.rst b/doc/changelog.rst index 4ef058ea..82259ed9 100644 --- a/doc/changelog.rst +++ b/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 ` where unnormalized paths would fail to be read. diff --git a/doc/includes/references.h b/doc/includes/references.h index 520785a7..97c61cd9 100644 --- a/doc/includes/references.h +++ b/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); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 10c0a6e6..91d59856 100644 --- a/include/mujoco/mujoco.h +++ b/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. -MJAPI void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M); +// Convert sparse inertia matrix into full (i.e. dense) matrix. +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); diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 05dfa362..97c7fb79 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -591,15 +591,12 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def( pymodule, - [](const raw::MjModel* m, Eigen::Ref dst, - Eigen::Ref M) { - if (M.size() != m->nM) { - throw py::type_error("M should be of size nM"); - } + [](const raw::MjModel* m, const raw::MjData* d, + Eigen::Ref dst) { 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( pymodule, diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 5fc5cf92..800ed67c 100644 --- a/python/mujoco/introspect/functions.py +++ b/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( diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 73e62190..1c1874e6 100644 --- a/src/engine/engine_support.c +++ b/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) { - 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++; - } - } +void mj_fullM(const mjModel* m, const mjData* d, mjtNum* dst) { + mju_sym2dense(dst, d->M, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind); } diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index cf112cb0..0f447608 100644 --- a/src/engine/engine_support.h +++ b/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); diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index df97cd02..f689105a 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/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 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 M2(nv*nv); - mj_fullM(m, M2.data(), d->qM); + mj_fullM(m, d, M2.data()); vector ten_J(nv); // tendon Jacobian vector ten_M(nv*nv); // tendon inertia @@ -681,7 +681,7 @@ TEST_F(CoreSmoothTest, FactorI) { // dense M matrix vector 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 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++) { diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 471c4540..25221e0d 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -848,7 +848,7 @@ TEST_F(DerivativeTest, LinearSystemInverse) { // expect that acceleration derivatives are the mass matrix vector 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 diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 30bb5dfe..cd753e47 100644 --- a/test/engine/engine_support_test.cc +++ b/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 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 M_CSR(nv * nv); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 49b99f3c..c05b5c4d 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/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); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 36cc5dfa..723092d3 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/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) { diff --git a/wasm/codegen/generators/constants.py b/wasm/codegen/generators/constants.py index 7a826e09..6a6ba85a 100644 --- a/wasm/codegen/generators/constants.py +++ b/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": """