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
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);
}
}