chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Migrate mjd_inverseFD mass Jacobian from qM to M

Browse Source 
PiperOrigin-RevId: 942268237
Change-Id: I0ecfe161867ce9930cd6366d778077df2cd3197f
This commit is contained in:
Yuval Tassa
2026-07-03 15:35:16 -07:00
committed by Copybara-Service
parent 4b345457a8
commit 7e9ac58ff9
12 changed files with 24 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+1 -1
View File
@@ -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).
doc/APIreference/functions_override.rst
+1 -1
View File
@@ -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).
doc/changelog.rst
+3
View File
@@ -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)
------------------------------
include/mujoco/mujoco.h
+2 -2
View File
@@ -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
python/mujoco/bindings_test.py
+1 -1
View File
@@ -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,
python/mujoco/functions.cc
+2 -2
View File
@@ -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,
python/mujoco/introspect/functions.py
+1 -1
View File
@@ -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(
src/engine/engine_derivative_fd.c
+6 -6
View File
@@ -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);
}
}
test/engine/engine_derivative_test.cc
+3 -3
View File
@@ -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<mjtNum> DfDq(nv * nv);
vector<mjtNum> DfDv(nv * nv);
@@ -793,7 +793,7 @@ TEST_F(DerivativeTest, LinearSystemInverse) {
vector<mjtNum> DsDq(nv * ns);
vector<mjtNum> DsDv(nv * ns);
vector<mjtNum> DsDa(nv * ns);
vector<mjtNum> DmDq(nv * nM);
vector<mjtNum> 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<mjtNum> DmDq_expect(nv * nM, 0);
vector<mjtNum> DmDq_expect(nv * nC, 0);
EXPECT_THAT(DmDq, Pointwise(DoubleNear(eps), DmDq_expect));
mj_deleteData(data);
wasm/codegen/generated/bindings.cc
+1 -1
View File
@@ -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());
}
wasm/codegen/generators/constants.py
+1 -1
View File
@@ -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);
wasm/tests/bindings_test.ts
+2 -2
View File
@@ -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(