From 7e9ac58ff90fd521aaf336220bcae23f903f2756 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 3 Jul 2026 15:35:16 -0700 Subject: [PATCH] Migrate `mjd_inverseFD` mass Jacobian from `qM` to `M` PiperOrigin-RevId: 942268237 Change-Id: I0ecfe161867ce9930cd6366d778077df2cd3197f --- doc/APIreference/functions.rst | 2 +- doc/APIreference/functions_override.rst | 2 +- doc/changelog.rst | 3 +++ include/mujoco/mujoco.h | 4 ++-- python/mujoco/bindings_test.py | 2 +- python/mujoco/functions.cc | 4 ++-- python/mujoco/introspect/functions.py | 2 +- src/engine/engine_derivative_fd.c | 12 ++++++------ test/engine/engine_derivative_test.cc | 6 +++--- wasm/codegen/generated/bindings.cc | 2 +- wasm/codegen/generators/constants.py | 2 +- wasm/tests/bindings_test.ts | 4 ++-- 12 files changed, 24 insertions(+), 21 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 7793b552..6d194abb 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3198,7 +3198,7 @@ using finite-differencing. These matrices and their dimensions are: ``DsDq``, :math:`\partial s / \partial q`, ``nv x nsensordata`` ``DsDv``, :math:`\partial s / \partial v`, ``nv x nsensordata`` ``DsDa``, :math:`\partial s / \partial a`, ``nv x nsensordata`` - ``DmDq``, :math:`\partial M / \partial q`, ``nv x nM`` + ``DmDq``, :math:`\partial M / \partial q`, ``nv x nC`` - All outputs are optional (can be NULL). - All outputs are transposed relative to Control Theory convention (i.e., column major). diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index e091c5e0..bd673a6f 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -874,7 +874,7 @@ using finite-differencing. These matrices and their dimensions are: ``DsDq``, :math:`\partial s / \partial q`, ``nv x nsensordata`` ``DsDv``, :math:`\partial s / \partial v`, ``nv x nsensordata`` ``DsDa``, :math:`\partial s / \partial a`, ``nv x nsensordata`` - ``DmDq``, :math:`\partial M / \partial q`, ``nv x nM`` + ``DmDq``, :math:`\partial M / \partial q`, ``nv x nC`` - All outputs are optional (can be NULL). - All outputs are transposed relative to Control Theory convention (i.e., column major). diff --git a/doc/changelog.rst b/doc/changelog.rst index 20b07d9c..a505f3c4 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -22,6 +22,9 @@ General :class: attention - Return type of :ref:`mj_encode` and the :ref:`mjfEncode` callback changed from ``int`` to ``mjtSize`` (64-bit). + - Switched :ref:`mjd_inverseFD` to use the CSR-format ``mjData.M`` representation instead of the legacy ``mjData.qM`` + for the mass matrix derivative. This changes the shape of the ``DmDq`` parameter from ``(nv x nM)`` to + ``(nv x nC)``. Version 3.10.0 (June 22, 2026) ------------------------------ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 62f476bc..e734aaac 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1483,10 +1483,10 @@ MJAPI void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtBool flg // DsDq: (nv x nsensordata) // DsDv: (nv x nsensordata) // DsDa: (nv x nsensordata) -// DmDq: (nv x nM) +// DmDq: (nv x nC) // single-letter shortcuts: // inputs: q=qpos, v=qvel, a=qacc -// outputs: f=qfrc_inverse, s=sensordata, m=qM +// outputs: f=qfrc_inverse, s=sensordata, m=M // notes: // optionally computes mass matrix Jacobian DmDq // flg_actuation specifies whether to subtract qfrc_actuator from qfrc_inverse diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index d7cb142f..6bd5d02a 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1508,7 +1508,7 @@ Euler integrator, semi-implicit in velocity. ds_dq = np.zeros((self.model.nv, self.model.nsensordata), dtype=DTYPE) ds_dv = np.zeros((self.model.nv, self.model.nsensordata), dtype=DTYPE) ds_da = np.zeros((self.model.nv, self.model.nsensordata), dtype=DTYPE) - dm_dq = np.zeros((self.model.nv, self.model.nM), dtype=DTYPE) + dm_dq = np.zeros((self.model.nv, self.model.nC), dtype=DTYPE) mujoco.mjd_inverseFD( self.model, self.data, diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index b8673a0e..f12c2630 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -1640,8 +1640,8 @@ PYBIND11_MODULE(_functions, pymodule, pybind11::mod_gil_not_used()) { throw py::type_error("DsDa should be of shape (nv, nsensordata)"); } if (DmDq.has_value() && - (DmDq->rows() != m->nv || DmDq->cols() != m->nM)) { - throw py::type_error("DmDq should be of shape (nv, nM)"); + (DmDq->rows() != m->nv || DmDq->cols() != m->nC)) { + throw py::type_error("DmDq should be of shape (nv, nC)"); } return InterceptMjErrors(::mjd_inverseFD)( m, d, eps, flg_actuation, diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 9325d3d4..4a023ce4 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -9533,7 +9533,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ nullable=True, ), ), - doc='Finite differenced Jacobians of (force, sensors) = mj_inverse(state, acceleration) All outputs are optional. Output dimensions (transposed w.r.t Control Theory convention): DfDq: (nv x nv) DfDv: (nv x nv) DfDa: (nv x nv) DsDq: (nv x nsensordata) DsDv: (nv x nsensordata) DsDa: (nv x nsensordata) DmDq: (nv x nM) single-letter shortcuts: inputs: q=qpos, v=qvel, a=qacc outputs: f=qfrc_inverse, s=sensordata, m=qM notes: optionally computes mass matrix Jacobian DmDq flg_actuation specifies whether to subtract qfrc_actuator from qfrc_inverse', # pylint: disable=line-too-long + doc='Finite differenced Jacobians of (force, sensors) = mj_inverse(state, acceleration) All outputs are optional. Output dimensions (transposed w.r.t Control Theory convention): DfDq: (nv x nv) DfDv: (nv x nv) DfDa: (nv x nv) DsDq: (nv x nsensordata) DsDv: (nv x nsensordata) DsDa: (nv x nsensordata) DmDq: (nv x nC) single-letter shortcuts: inputs: q=qpos, v=qvel, a=qacc outputs: f=qfrc_inverse, s=sensordata, m=M notes: optionally computes mass matrix Jacobian DmDq flg_actuation specifies whether to subtract qfrc_actuator from qfrc_inverse', # pylint: disable=line-too-long )), ('mjd_subQuat', FunctionDecl( diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index 98fcc281..049651d9 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -598,10 +598,10 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtBool flg_cente // DsDq: (nv x nsensordata) // DsDv: (nv x nsensordata) // DsDa: (nv x nsensordata) -// DmDq: (nv x nM) +// DmDq: (nv x nC) // single-letter shortcuts: // inputs: q=qpos, v=qvel, a=qacc -// outputs: f=qfrc_inverse, s=sensordata, m=qM +// outputs: f=qfrc_inverse, s=sensordata, m=M // notes: // optionally compute mass matrix Jacobian DmDq // flg_actuation specifies whether to subtract qfrc_actuator from qfrc_inverse @@ -609,7 +609,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtBool flg_actuatio mjtNum *DfDq, mjtNum *DfDv, mjtNum *DfDa, mjtNum *DsDq, mjtNum *DsDv, mjtNum *DsDa, mjtNum *DmDq) { - int nq = m->nq, nv = m->nv, nM = m->nM, ns = m->nsensordata; + int nq = m->nq, nv = m->nv, nC = m->nC, ns = m->nsensordata; if (m->opt.integrator == mjINT_RK4) { mjERROR("RK4 integrator is not supported"); @@ -628,7 +628,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtBool flg_actuatio mjtNum *force = mjSTACKALLOC(d, nv, mjtNum); // force mjtNum *force_plus = mjSTACKALLOC(d, nv, mjtNum); // nudged force mjtNum *sensor = skipsensor ? NULL : mjSTACKALLOC(d, ns, mjtNum); // sensor values - mjtNum *mass = DmDq ? mjSTACKALLOC(d, nM, mjtNum) : NULL; // mass matrix + mjtNum *mass = DmDq ? mjSTACKALLOC(d, nC, mjtNum) : NULL; // mass matrix // save current positions mju_copy(pos, d->qpos, nq); @@ -636,7 +636,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtBool flg_actuatio // center point outputs inverseSkip(m, d, mjSTAGE_NONE, skipsensor, flg_actuation, force); if (sensor) mju_copy(sensor, d->sensordata, ns); - if (mass) mju_copy(mass, d->qM, nM); + if (mass) mju_copy(mass, d->M, nC); // acceleration: skip = mjSTAGE_VEL if (DfDa || DsDa) { @@ -702,7 +702,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtBool flg_actuatio if (DsDq) diff(DsDq + i*ns, sensor, d->sensordata, eps, ns); // row of inertia Jacobian - if (DmDq) diff(DmDq + i*nM, mass, d->qM, eps, nM); + if (DmDq) diff(DmDq + i*nC, mass, d->M, eps, nC); } } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 86a18c24..00211e27 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -785,7 +785,7 @@ TEST_F(DerivativeTest, LinearSystemInverse) { int nv = model->nv; int ns = model->nsensordata; - int nM = model->nM; + int nC = model->nC; vector DfDq(nv * nv); vector DfDv(nv * nv); @@ -793,7 +793,7 @@ TEST_F(DerivativeTest, LinearSystemInverse) { vector DsDq(nv * ns); vector DsDv(nv * ns); vector DsDa(nv * ns); - vector DmDq(nv * nM); + vector DmDq(nv * nC); // call mj_forward to get accelerations at initial state mj_forward(model, data); @@ -847,7 +847,7 @@ TEST_F(DerivativeTest, LinearSystemInverse) { EXPECT_THAT(DsDa, Pointwise(DoubleNear(eps), DsDa_expect)); // expect that mass matrix derivatives are zero - vector DmDq_expect(nv * nM, 0); + vector DmDq_expect(nv * nC, 0); EXPECT_THAT(DmDq, Pointwise(DoubleNear(eps), DmDq_expect)); mj_deleteData(data); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 322db2ea..88825c39 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -2057,7 +2057,7 @@ void mjd_inverseFD_wrapper(const MjModel& m, MjData& d, mjtNum eps, mjtBool flg_ CHECK_SIZE(DsDq, m.nv() * m.nsensordata()); CHECK_SIZE(DsDv, m.nv() * m.nsensordata()); CHECK_SIZE(DsDa, m.nv() * m.nsensordata()); - CHECK_SIZE(DmDq, m.nv() * m.nM()); + CHECK_SIZE(DmDq, m.nv() * m.nC()); mjd_inverseFD(m.get(), d.get(), eps, flg_actuation, DfDq_.data(), DfDv_.data(), DfDa_.data(), DsDq_.data(), DsDv_.data(), DsDa_.data(), DmDq_.data()); } diff --git a/wasm/codegen/generators/constants.py b/wasm/codegen/generators/constants.py index acfcac22..3aeb94f7 100644 --- a/wasm/codegen/generators/constants.py +++ b/wasm/codegen/generators/constants.py @@ -660,7 +660,7 @@ FUNCTION_BOUNDS_CHECKS: Dict[str, str] = { CHECK_SIZE(DsDq, m.nv() * m.nsensordata()); CHECK_SIZE(DsDv, m.nv() * m.nsensordata()); CHECK_SIZE(DsDa, m.nv() * m.nsensordata()); - CHECK_SIZE(DmDq, m.nv() * m.nM()); + CHECK_SIZE(DmDq, m.nv() * m.nC()); """.strip(), "mjd_subQuat": """ CHECK_SIZE(qa, 4); diff --git a/wasm/tests/bindings_test.ts b/wasm/tests/bindings_test.ts index 7e259af8..0030c092 100644 --- a/wasm/tests/bindings_test.ts +++ b/wasm/tests/bindings_test.ts @@ -1305,7 +1305,7 @@ describe('MuJoCo WASM Bindings', () => { const nv = model!.nv; const nsensordata = model!.nsensordata; - const nM = model!.nM; + const nC = model!.nC; const dfDq = new mujoco.DoubleBuffer(nv * nv); const dfDv = new mujoco.DoubleBuffer(nv * nv); @@ -1313,7 +1313,7 @@ describe('MuJoCo WASM Bindings', () => { const dsDq = new mujoco.DoubleBuffer(nv * nsensordata); const dsDv = new mujoco.DoubleBuffer(nv * nsensordata); const dsDa = new mujoco.DoubleBuffer(nv * nsensordata); - const dmDq = new mujoco.DoubleBuffer(nv * nM); + const dmDq = new mujoco.DoubleBuffer(nv * nC); try { mujoco.mjd_inverseFD(